Active Modeling and Compensation for the Hysteresis of a Robotic Flexible Ureteroscopy

Hysteresis may be one of the most critical issues degrading the performance of the automatic operation of robotic flexible ureteroscopy, i.e., a robotic device handles the flexible ureteroscopy tracking a target automatically. In this paper, we proposed an active-model based scheme to estimate the hysteresis in real time. Firstly, we used the Coleman-Hodgdon model to describe the backlash hysteresis property of the flexible scope, and constructed the reference model of the bending angle. Secondly, a modeling error was introduced into the reference model, and the UKF-based estimator was used to estimate both the bending angle and the modeling error actively. Finally, we designed the hysteresis compensation based on this active estimation enhanced model. Extensive experiments were conducted on the self-built robotic ureteroscopy. The experimental results with the active modeling enhancement were presented and compared with those without it, to demonstrate the improvements achieved by the proposed scheme.


I. INTRODUCTION
Besides the minimal invasive surgery (MIS), the natural orifice transluminal endoscopic surgery (NOTES) has been developing rapidly in the last decades [1]- [4]. Compared to the open surgery, both NOTES and MIS have decreased surgical morbidity and postoperative recovery time benefited from the smaller or even no incision [5]. However, there are still difficulties while the doctors performing NOTES operations, and how to accurately navigate and locate the flexible endoscope inside the ''closed'' human organs may be one of the most significant challenges.
As shown in Fig. 1, flexible ureteroscope based operation, namely, ureteroscopy, is one of the most popular NOTES that is being adopted by more and more patients and doctors. A flexible ureteroscope is an instrument that is used to pass through and see inside of the urinary tract. Usually, it is used in ureteroscopies to locate and shatter kidney stones. During the ureteroscopy, a ureteral access sheath is first inserted into the urethra, then the surgeon will operate the flexible ureteroscope passing the sheath into the bladder and then the ureter and finally the kidneys. This procedure doesn't require The associate editor coordinating the review of this manuscript and approving it for publication was Okyay Kaynak . an incision, making it less invasive and reducing the risk of infection for the patient.
As Fig. 2 shows, normally there are 3 controllable degreeof-freedoms (DOFs) on a flexible ureteroscope, namely backand-forth of ξ , rotation of β and tip-bending of α, which can be operated via the handle shown in Fig. 2. The tip-bending of the ureteroscope is driven by a very thin and solid wire going through the scope's epidermis and fixed on the two rings of the handle. When the wire is pulled by the surgeon, the tip-part will bend correspondingly; On the contrary, when the wire is loosened, the tip-part will straighten. During ureteroscopies, the surgeon operates the 3-DOFs and focuses the scope tip on the target point, a small stone in kidney for example. Actually, it is not easy for a surgeon to control the flexible scope and locate its tip accurately on the target.
Instead of a surgeon, a robotic device can also be constructed to operate the 3 DOFs of the scope via the handle, while the surgeon controls the robot remotely [6]. However, a master-slave-type robotic system also depends on human surgeon's experiences for the accurate allocation of the scope tip. In order to make the robot focus on the target point automatically, we have to develop a kinematic model of the flexible ureteroscope, and the accuracy of the tip allocation will closely depend on the accuracy of the kinematics [7].  Various model identification methods of continuum robots have been carried out in the studies [8]- [10]. Compared with the traditional rigid-link robots, cable-driven continuum surgery robots have hysteresis, elasticity and other nonlinear properties which are difficult to be modeled. The uncertainties of cable-driven robot were analyzed by the tendon-sheath mechanism (TSM) in [11], [12], where static models were proposed at both sliding and pre-sliding regimes to predict the nonlinear hysteresis of TSM. In [13], the ''dead zone'' was analyzed with respect to the friction between tendon and sheath, where the hysteresis of the friction and position was respectively modeled in the form of Bouc-Wen and Coleman-Hodgdon. About the kinematics of flexible scope, Zhang et al. approximated the bending angle of ureteroscope into piecewise function [14]- [16], and two curves of flexion angle corresponding to the bi-directional bending knob angle were given out. These two curves were respectively divided into 3 and 5 segments, and a series of linear models were obtained by fitting each segment of the curves into a strain line. Similarly, the studies carried in [17] also used a static model with linearization. But the linearization, as well as the uncaptured uncertainties, might degrade the accuracy of the kinematic model.
In this paper, we proposed an active modeling technique for the flexible ureteroscope. We used the continuous backlash-like hysteresis model, namely, Coleman-Hodgdon model [18], to describe the hysteresis of flexible ureteroscope, and constructed the kinematic model based on the Denavit Hartenberg (D-H) method. The parameters inside this model were identified offline by the experimental data with respect to a specific motion state. Unscented Kalman Filter (UKF) was then used to actively estimated the hysteresis and other modeling errors that did not include in the identified reference model [19]. Finally, an active model enhanced PD controller (AME-PD) was designed to control the bending angle of the ureteroscope tracking a given trajectory.
The main contributions of this paper are summarized as follows: • The active modeling scheme, which composed of a normal D-H based offline reference model and an active estimation of modeling error, was proposed for the flexible ureteroscope.
• The Unscented Kalman Filter was used as the active estimator in the proposed scheme. An active model enhanced PD controller was also designed to control the tip-part's bending angle tracking a given trajectory automatically.
• Extensive experiments were conducted on a robotic ureteroscope testbed. The experimental results were compared with those without the active estimation, to demonstrate the achievements obtained by the proposed scheme.
• Without losing the generality, the proposed scheme might provide a feasible way for the active modeling and tip-tracking control of other flexible scopes, to decrease the dependency of a surgery robot on the remote control of surgeons. The rest of this paper is organized as follows. In Section II, a novel kinematic model to describe the hysteresis of the ureteroscope's tip-part is established. The UKF estimator is designed and the active modeling technology is described in Section III, and the tracking control strategy is proposed in Section IV. Finally, the experimental results were presented and analyzed in Section V, following the conclusion in Section VI.

II. KINEMATICS OF FLEXIBLE URETEROSCOPE A. GEOMETRIC STRUCTURE AND KINEMATICS
The geometric structure of the ureteroscope is shown in Fig. 3 and the symbol definitions are introduced in Table 1. Here, we assume that the bending part will always be a circular arc.
Thus, the geometric structure of the ureteroscope tip can be described by Fig.3, and we have and where W R B represents the rotation transformation of frame {B} rotating clockwise β about the frame {W}. The homogeneous transformation t w→c can be obtained from (1), (2) as follows where t w→b = [0, 0, ξ ] T is the translation transformation of the frame {B} in the frame of {W}. Define the forward kinematic as and with the geometric structure, we have thus the translation J can be expressed as The kinematic model can be obtained by differentiating position vector t w→c with respect to time t as follows where Thus, we get the kinematic model of the ureteroscope as (8).

B. BACKLASH HYSTERESIS MODEL
We could identify the parameters in (8) offline by the measurements with respect to a specific motion state. However, we have seen in our experiments that the output of (8) did not meet the real states accurately (see Fig. 6 of the experimental results). This is mainly due to the hysteresis. The backlash hysteresis nonlinearity of the bending angle of tip-part is due to nonlinear friction of the wire and the elasticity of the ureteroscope. For positive velocity, the bending angle α increases with the pulling distance δl. In the transition phase, the output keeps its previous value at the transition phase.
To capture such a backlash hysteresis phenomena of the flexible ureteroscope, a continuous Coleman-Hodgdon model is used. This model describe a standard backlash hysteresis nonlinearity, as written bẏ where the parameters A, B and C are respectively constant coefficient and must satisfy A > 0, , δl is the model value of bending angle and pulling distance, and we haveᾱ where the point O Backlash = (O δl , O α ) is the origin point of backlash phase, which is the central point of the hysteresis loop in the phase of kinematic.δl(t) andᾱ(t) are the phase transitions of δl(t), α(t) respectively.
By (10), the backlash hysteresis of the bending angle α in model (8) is described. This hysteresis model has static parameters and a fixed hysteresis loop. The property of the relation between the input δl(t) and output α(t) is that they are monotonically increasing and decreasing between the two extrema. A large value of A or a small value of B means that the hysteresis loop is slim. The hysteresis loop has a steep inclination, when C is large.

III. ACTIVE ESTIMATION OF MODELING ERROR
The hysteresis model of (10) is static with constant parameters, and its accuracy may be degraded by the time-varying uncertainties of the real flexible scope. In order to handle this, we propose in this section a modeling error to describe the uncertainties in (10), and use the unscented Kalman filter to estimate it actively. Thus, we re-write (10) as where Y (t) is the system output which is measurable, U (t) is the process noise involved in the model error and V (t) is the observation noise from the system. All the uncertainties in (12) caused by model error can be considered as additive process noise. Thus, when v δl (t) = 0, the modeling errors of (12) can be modeled by where f h (t) ∈ R represents the model error, α(t) is the state of the reference model (10),α(t) is the actual state, h(t) is assumed to be the process noise actuating the model errors to update, the actual model is defined as hereα(t) represents the measurement of bending angel and α(t) =α(t) − O α . Compared with the system's sampling frequency (often 50 Hz or higher), model error f h (t) can be considered as a slowly time-varying value, and Then the discrete description of equation (14) is where v δl(k) = (δl k − δl k−1 )/T s is the discrete expression of pulling velocity; T s is the sampling time; In consideration of the nonlinearity and coupling of model (15), an UKF is used as an estimator to estimate state X k in real time based on the state-space equation (16). The estimator is designed as wherein Q k is the covariance matrix of process noise [U k , h k ], R k is the covariance matrix of measurement noise V k , k−1 is the confidence field of the state vector X k−1 , it is related to the Cholesky decomposition matrix L of confidence matrix P k−1 , X k|k is the estimation of state vector based on confidence field k|k−1 , P k|k is the update of confidence matrix P k−1|k−1 . k−1 (i), k|k−1 (i) represents the i-th column vector of k−1 , k|k−1 . k|k−1 (i, j), k−1 (i, j) means the element at (i, j) of the matrix k|k−1 , k−1 , constant n is the dimension of the state vector X k .
Thus, we can estimate X k|k from (18) and obtain the estimation of both theα k and f h(k) of (17).

IV. TRACKING CONTROL
The scheme of tracking control is described in Fig. 4. A reference model of the scope needs to be selected and identified first, and then a nominal controller needs to be designed based on this reference model. The system state is estimated in real time by the active modeling technology and input into the compensation strategy unit, which generates the compensation to reduce tracking error. In this structure, we do not need a precise reference model, and all the uncertainties are left to the UKF estimator and the compensation strategy.
In this section, we design an AME-PD control method of the bending angle α with the strategy shown in Fig. 4. With the estimator of (17), the real kinematic of scope-tip could be estimated in real time. The applied pulling velocityδl k can be expressed as where v δlnom(k) is the nominal control, and v δlnom(k) is the compensation designed by the compensation strategy as Fig. 4 shows. If the measurement bending angleα k can track the target trajectory α d(k) precisely, it obtains In this paper, the nominal input v δlnom(k) is designed by PD controller and the input compensation v δlcom(k) is designed by compensation strategy in Fig. 4. Define the tracking error of the bending angle α(k) as e k = α d(k) −α k , and the nominal control where k p , k d are the parameters of PD controller, thus the actual bending angleα k+1 is predicted to be, Define the predicted tracking error In this paper, with the idea of PD controller, the predicted tracking error from (18) is calculated in the compensation strategy. Thus, the active model-based compensation is designed as where k c is a constant proportion parameter. Thus, the final The stability of this control scheme is closely dependent on that of the normal PD control. From (23) we can see that the enhancement part is a model-predictive open loop compensation, while theα k+1 is the model predictive state as (22). So, this compensation will not influence the stability of the normal PD scheme [20].

V. DEVICE SET UP AND EXPERIMENTAL ANALYSIS A. DEVICE SET UP AND BENDING EXPERIMENTS
In this section, hardware experimental results are provided to validate the proposed scheme. The self-built robotic device held the ureteroscope and could pull or push the two rings on the handle at a given speed as shown in Fig. 5. The flexible ureteroscope operated by the robot was a 1-wire 8 F / 70 cm flexible micro-endoscope from PolyDiagnost (Lumenis, Santa Clara, CA). It composed of a single-used flexible catheter and a reusable fiber optic bundle. Two cameras (TIS GigE DFK33GP1300) were used in our experiments to measure the bending angel α of the ureteroscope. An Industrial Personal Computer (IPC, ADVANTECH ARK-3500 (Intel(R) Core(TM) i7-3610QE 2.3GHz 16.00G) served to collect data from the encoders on the robot and the cameras, and execute the control algorithms proposed.
The first experiment we conducted was to test the hysteresis of the scope. The velocity of δl was set to three constant values respectively, i.e., v δl = {0.125, 0.075, 0.025} mm/s and the experimental results were shown in Fig. 6, from which we can see there exists hysteresis clearly between the bending angle α and the wire-pulling-distance δl.
Further experiments were also conducted while the wirepulling-velocity was set as triangular wave. The whole distance of δl was divided into three segments, i.e., δl ∈ [0, 3] mm, δl ∈ [3, 6] mm and δl ∈ [0, 6] mm, to demonstrate the ''length-dependent'' hysteresis of the scope. Fig. 7 showed the results of the output of kinematic model (8) and the bending angle measurement, and Fig. 7(a), Fig. 7(b) and Fig. 7(c)   FIGURE 6. the relationship between bending angle α and the pulling distance δl . From Fig. 7 we can see the significant difference between them due to the influence of the unmodeled hysteresis.

B. IDENTIFICATION OF HYSTERESIS MODEL
In this study, we used genetic algorithm (GA) to identify the parameters of the A, B and C in (10). To do this, we defined the mean square error (MSE) as where i is the sampling index and N is the total number of sampling points from hardware experiment, α Real (t) is the actual bending angle measurement, α Model (t) is the value calculated by reference model (10). The MSE is used as fitness function in GA and it represents the mean square error between measurement and the output of the model (10).  Fig. 8 demonstrated the results of the model output of (10) and the real measurement of α with respect to the three distance segments, where we can clearly see the modeling errors between them. Fig. 10 showed the repeatability of the nominal hysteresis model without the active estimation in different segments.
Besides the figures demonstrating the model output and the real measurements, we defined the following two indexes to quantify the errors between them.
where α Real and α Model are the reference model output of (10) and the real measurement of the bending angle of the bending angle; Err Model is the mean value of Err Model and N is the number of sampling points. The Err Model and Var Model were calculated using the data of Fig. 8, and the results were listed in Table 2. Both Fig.8 and Table 2 prove that the reference model can only represent the tendency of the bending angle, but cannot meet the real measurement very well. The model error is clearly time varying and different when the pulling distance changes in different segments. VOLUME 8, 2020  (16) by the data of Fig. 9.

C. ACTIVE MODEL ERROR ESTIMATION
By setting the initial values as: the modeling error enhanced estimator was experimentally tested under the same scenario as that of Section V. B. Fig. 9 demonstrated the results, where we can see the errors between the estimator output and the real measurement had been reduced significantly. Fig. 11 showed the repeatability of the nominal hysteresis model with the active estimation. Two indexes were also defined as (30) and (31), and the values of Fig. 9 were calculated and listed in Table 3. Table 4 showed the ratios between the values in Table 2 and 3, which indicated the improvement achieved by the active estimation. We can see that the |Err Model | has been reduced by nearly 97%, and the Var Model has been reduced by more than 99.5%.
where α EstM and α Real are the estimated model output of (16) and real measurement of the bending angle α; Err EstM is the mean value of Err EstM and N is the number of sampling points.

D. ACTIVE MODEL ENHANCED PD CONTROL
In order to test the performance of AME-PD controller designed in Section IV, the tracking trajectory α d (t) is set to α d (t) = −50 cos( π 50 t) + 80 (32) the parameters of PD and AME-PD controller were set manually to k p = 0.02, k d = 0.05, k c = 0.06, in the goal of achieving fast and accurate response. The tracking performance under the PD control and the AME-PD control were presented in Fig. 12, and the tracking errors were plotted in Fig. 13. Again, we defined the two index as: where α d(k) andα k are the discrete form of α d (t) andα k measured by visual capture. Err is the mean value of tracking error Err, and N is the number of sampling points. The corresponding values were listed in Table 5, where we can see that the tracking error had been improved by more than 70% after introducing the simple ''estimator predictive enhancement'' of k c into (24).

VI. CONCLUSION
In this paper, the active modeling scheme, which is composed of the Coleman-Hodgdon backlash model and the UKF-based model error estimation, was proposed for the robotic flexible ureteroscopy. This intended to obtain an accurate description, and also a compensation control of the hysteresis of the flexible scope. Compared with the existed methods such as the hysteresis model with adaptive parameters, the proposed scheme might be more feasible for real implementation. Besides the simple structure and good performance we had shown, the UKF-based active estimation is a kind of 'data-driven', and the Kalman Filter type algorithm has been extensively applied on real systems and its robustness and feasibility have been verified.
In additional to the flexible ureteroscopy we had tested, the proposed scheme might be a general approach for the active modeling and tracking control of other flexible scopes. And besides the simple PD-type ''active model predictive'' compensation introduced in this paper, a nonlinear control was under design and test for near future publication.