UKF-Based Motion Estimation of Cable-Driven Forceps for Robot-Assisted Surgical System

This paper presents an Unscented Kalman Filter (UKF)-based method to achieve high-precision motion estimation of cable-driven forceps for a robot-assisted surgical system. We analyze the operational/working principle of revolute joints of a 3-degree-of-freedom (3-DOF) cable-driven surgical manipulator. Then a gripper jaw is selected as a representative joint, which is actuated by a single-motor cable-driven mechanism with a reset spring. The corresponding system dynamics comprehend the mass, elasticity, damping, and friction of steel cables. By using the displacement and velocity of reset cable and the rotation angle of motor as observations, the motion estimation model based on UKF is derived. The estimation accuracy is verified experimentally, with the errors of absolute and root-mean-square (RMS) of less than 0.5 deg and 0.2 deg respectively. By comparisons with the least square methods (LSMs), the installation strategy of only one displacement sensor on the reset cable is determined, which is conducive to further refinements of the mechanism. Furthermore, the external force loading experiments are performed, with the RMS estimation error of less than 0.5 deg for the external force of no more than 250 g applied on the tip of the gripper jaw. These experimental results validate the motion estimation accuracy of cable-driven forceps, without requiring sensors on the end joints or slender tool shaft of surgical instruments.


I. INTRODUCTION
Robot-assisted minimally invasive surgery which features less pain, shortened hospital time, improved postoperative recovery, and reduced risk of infection and surgeon's fatigue when compared to the traditional open surgery, has received considerable attention and developed rapidly in last two decades [1], [2]. To avoid additional injury to patients and improve the safety, surgical instruments should have a small dimension. Furthermore, the control accuracy of both the position and force acting on the patient's tissues or organs should be sufficiently high. Fortunately, the cabledriven mechanism has its distinct advantages in simplifying the structure and reducing the size of the instrument.
The associate editor coordinating the review of this manuscript and approving it for publication was Seung-Hyun Kong .
Meanwhile, it can separate motors and end effectors, which is beneficial to reduce the weight the terminal mechanism in the patient [3], [4]. Moreover, the flexible cables can effectively achieve a broad motion range and high flexibility of end joints. The high tension of cables enables it possible to possess no mechanical transmission clearance [5], [6]. These advantages make the cable-driven technology widely used in surgical instruments.
However, for cable-driven surgical instruments, current commercialized sensors are not applicable for installations at the end joints because of the strictly limited size. Highprecision control of end effectors is challenging without any sensory feedback information. Cable-driven surgical instruments have a standard way of open-loop control based on the kinematics [7]- [9], and the control accuracy of their end effectors has a significant dependence on the system modeling Differently, a neural network in [10] was developed to acquire the inverse dynamics of a pneumatic servo system as a feedforward controller to control surgical instruments, and then to further estimate external forces on the tip of forceps [11]. Although the position accuracy is limited by friction caused by wire ropes the consistency of master-slave control is well demonstrated. An image-based soft robotic control [12] has a significant error for a long time before convergence Additionally, in [13], [14], the compensations about the backlash of the wire ropes driving pitching and distal rolling joints were performed and then applied to the motor control Nevertheless, the actual results are still accompanied by the maximum angular errors of about 2.0 and 4.5 deg respectively Similarly, the observers based on a symmetric backlash hysteresis model were proposed in [15] in a system feedback controller. Based on this, the absolute error of system position control can be reduced to less than 3.3 deg With the measurements of potentiometers as system feedback, a computer-based position controller was designed in [16], which enabled repeatability of less than 1.0 deg for rigid-linkage mechanisms. In addition, some surgical forceps driven by pneumatic cylinders were also developed [17]- [19], whose position estimation and control were performed based on the pressure change of pneumatic cylinders. Although the feedback controller established in the drive unit [20]- [23] could realize the closed-loop motion control, the position transmission loss caused by the long distance between the drive unit and the end joints is still a severe fact that causes a significant error. Different from the above approaches, a linear parametervarying model to identify the nonlinear motion of a finger joint was developed in [24]. Besides, in [25], a joint angle estimator was also designed for cable-driven surgical forceps. However, the linear identification model has limitations in the presentation of the nonlinear characteristics of cabledriven systems. In order to approximate the system nonlinearity, the Unscented Kalman Filter (UKF) method was adopted to explore the motion estimation of cable-driven end joints [26], [27], and further implemented the motion control and external forces estimation [28], [29]. These studies have revealed that the joint motion estimation is critical for the high-precision motion control of surgical instruments when there are no sensors installed on the end joints or slender tool shaft. However, the nonlinearity of cables has always been a difficulty in improving the motion estimation accuracy for these cable-driven systems.
We present a driving mechanism scheme of cable-driven surgical forceps, where each end joint is driven only by a motor and a reset spring. The main contributions can be summarized as follows: 1) By fusing a nonlinear error compensation model into the UKF algorithm, the motion estimation model of cable-driven forceps is derived, where only the displacement and velocity of reset cable and the rotation angle of the motor are utilized as observations.
2) The estimation experiments and the comparisons with the least square methods (LSMs) validate the improved accuracy of the UKF method for loaded gripper jaw.

3)
The installation strategy of only a displacement sensor on the reset cable is further determined, which effectively simplifies the mechanism. Additionally, this estimation method can get rid of the dependence on sensors on the end joints or slender tool shaft of surgical instruments, which is desirable in actual surgeries.
The rest of this paper is organized as follows. In Section II, the principle of a classic 3-DOF cable-driven surgical instrument is analyzed, and a driving mechanism scheme with a gripper jaw is described. Section III presents the complete dynamics modeling of the cable-driven system and the motion estimation modeling based on UKF. In Section IV, the motion estimation experiments are performed, and the comparisons with LSMs are carried out. Finally, the conclusions and future work are followed in Section V.

A. OPERATIONAL PRINCIPLE
The robot-assisted surgical system is a highly integrated master-slave robot system, which mainly includes surgeon master control system, mechanical arm system, master-slave control console, and imaging system [30]. As shown in Fig. 1, a mechanical arm is furnished with a 3-DOF cable-driven manipulator The instrument includes a drive unit, a slender tool shaft, and end joints. The movement of end joints mainly focuses on pitching, yawing, and gripping. The slender tool shaft is mainly responsible for the disturbance-free transmission of flexible cables. The drive unit can be regarded as a mixed system, integrating servo motors and transmission mechanisms. The operational principle of the 3-DOF manipulator is shown in Fig. 2. Each revolute joint is driven  by two cables connected to a servo motor in the drive unit. In general, the end joints are assembled compactly without any interference between actuation cables due to the routing approach. Moreover, the cable tension can prevent mechanical clearance between the joints that often occurs in the roddriven mechanism. Consequently, if the kinematic coupling between end joints is not considered, the motion of each joint is independent and works on the same operational principle. In order to simplify mechanism analysis and avoid redundant descriptions, a gripper jaw is used as a representative end joint to evaluate the high-precision motion estimation method.

B. CABLE-DRIVEN MECHANISM WITH A GRIPPER JAW
In practice, installing sensors on each end joint or tool shaft of surgical instruments is challenging due to size constraints. The most feasible approach is to arrange sensors in the drive unit with relatively large physical space. Generally, there are three installation strategies of sensors on flexible cables, as shown in Fig. 3(a). Strategy-1 presents a motion control of surgical forceps that relies primarily on the motor information without any additional sensors installed on the cables. This strategy is usually accompanied by much motion loss, especially with the transmission mechanism and long cables. Therefore, for most existing electric-driven surgical instruments, this approach has been rarely used Strategy-2 mainly presents that only one sensor is arranged on the drive side or the reset side between the transmission mechanism and the revolute joint. In contrast, Strategy-3 is to install sensors on both sides to collect more motion information. In order to determine which of the Strategy-2 and Strategy-3 is more practical, further verification should be carried out. Then, a sample cable-driven mechanism with a gripper jaw previously proposed in [31] is employed, which is consistent with the Strategy-3.
As shown in Fig. 3(b), a gripper jaw is driven only by a motor and a reset spring, which can reduce the space occupancy in the drive unit compared with the dual-motor driving mode Moreover, it avoids the assembly difficulties and insufficient tension caused by connecting two cables on the same motor. In order to prevent relative sliding, a long cable is divided into a driving cable and a reset cable by a small pinch with the gripper jaw attached to it. A non-contact rotary encoder is linked to the shaft of the gripper jaw which is employed to measure its actual motion. It is worth noting that the rotary encoder is only used in the initial calibration of the cable-driven system and the motion estimation modeling because it does not exist in the actual surgical instruments.
Furthermore, it is only used as an actual reference in the verification experiments, which can also be replaced by other measuring devices. The optical encoders are placed at the ends of the cables close to the transmission mechanisms to measure dynamic information of cables. A winding wheel is machined as the transmission mechanism on the drive side. Besides the limit switch provides the origin position and safety protection.

III. DYNAMICS MODELING A. MODELING OF THE CABLE-DRIVEN SYSTEM
To describe the characteristics of the cable-driven system, the complete dynamics modeling is carried out in this section after containing the mass, elasticity, damping, and friction of the cables. The simplified schematic diagram of the system with a single grasper is shown in Fig. 4. The flexible steel cable is approximately simplified into a mass-spring-damper model. The displacements of the driving and reset cables are measured by the optical encoders 1 and 2, respectively. Since the mass of the gripper jaw is small and the rotating shaft is perpendicular to the ground, its gravity does not affect the system, which can be ignored herein.
In the initial position, the cables are in tension and the rotation angle θ of the gripper jaw is set to 0 deg. The tension of the driving and reset cables is equal to the initial pretension F pre , which can be written as: When the gripper jaw is driven to rotate forward at any angle θ = 0, the output angle of the motor shaft is expressed: where z i and ϕ m denote the reduction ratio and input angle of the motor, respectively. The cable in the transmission mechanism is so short that it can be regarded as rigid transmission. Then the displacement of optical encoder 1 can be written as follows: where R denotes the effective radius of the winding wheel. By simplifying the cable as a mass-spring-damper model, the tension of the driving cable is written as: where m dc , B d , and K d denote the equivalent mass, damping, and coefficient of elasticity of the driving cable, respectively.
x 1 andẍ 1 are the velocity and acceleration of the optical  encoder 1, respectively. y 1 ,ẏ 1 , andÿ 1 represent the displacement, velocity, and acceleration of the front end of the driving cable fixed on the gripper jaw, respectively. Similarly, the tension of the reset cable is written as: where m rc , B r , and K r denote the equivalent mass, damping, and coefficient of elasticity of the reset cable, respectively. x 2 ,ẋ 2 , andẍ 2 are the displacement, velocity, and acceleration of the optical encoder 2, respectively. y 2 ,ẏ 2 , andÿ 2 represent the displacement, velocity, and acceleration of the front end of the reset cable fixed on the gripper jaw, respectively. By combining the reset spring and the optical encoder 2, the expression is derived as follows: where m g2 , K sp , and F fr denote the mass of the optical encoder 2, the coefficient of elasticity of the reset spring, and the friction from the slide rail and guide pulleys, respectively. To facilitate the analysis we assume that the friction generated herein is the combination of Coulomb friction and viscous friction, which is the same as the friction model below f rv and f rc denote the coefficients of viscous friction and Coulomb friction, respectively. For the whole motion range, the winding length of cables around the gripper jaw is so short that its tiny deformation can be ignored. Therefore, the front-end displacements of the two cables are considered equal.
where r is the effective rotation radius. When the gripper jaw is separated as a module, the dynamic model can be obtained: where J e ,θ,θ, τ e , and τ ef denote the equivalent moment of inertia, angular velocity, acceleration, driving torque, and friction torque of the gripper jaw, respectively. f ev and f ec are the coefficients of viscous and Coulomb frictions, respectively.

B. MOTION ESTIMATION MODELING BASE ON UKF
The filtering theory is a method of estimating the system states by measuring observable signals and using statistical methods, according to a specific criterion [32]. The UKF is an extension of Kalman filtering algorithms, which performs a nonlinear estimation of a posterior probability density of system states [33]. Because of the nonlinearity and elasticity of long steel cables, the cable-driven surgical instrument should be considered as a nonlinear system. Herein, a nonlinear motion estimation model is established by fusing an error compensation model into the UKF algorithm to improve the VOLUME 8, 2020 motion accuracy of forceps further. It features that only the displacement and velocity of the reset cable measured by the optical encoder 2 are taken as observations except for the rotation angle of the motor. Because the optical encoders and the gripper jaw are mounted on the same cable throughout the whole system, their dynamic motion information are interrelated. Therefore, the variables θ,θ, andθ can be re-expressed by x 2 ,ẋ 2 ,ẍ 2 , and sign(ẋ 2 ) from the optical encoder 2 Then, (1), (4), (5) and (7) are substituted into (8) to get the expression as: Then, we combine (2), (3), and (9) to obtain another derivation: Similarly, (6) and (7) are substituted into (5) to get the following relation: In the actual system, the mass, damping, and friction of cables in (10) and (11) are far less than the moment of inertia J e and the mass of the optical encoder m g2 . Therefore, these variables have a much smaller proportion in the whole model than other variables, especially for θ and x 2 . Meanwhile, these values are not available by measuring devices. To simplify the model analysis, these variables are not considered temporarily in the initial estimation modeling. Then, the simplified model can be obtained as follows: Then, the formula (12) can be expressed in the form of state-space equation: where x = [ θθ x 2ẋ2 ] T denotes the state vector of the continuous-time system. u = ϕ m and y = [ x 2ẋ2 ] T denote the input matrix and output measurement matrix, respectively.
In (13), to take the displacement and velocity of the reset cable as the variables to be observed, the coefficient matrices are derived as: where each parameter can be known from the manual data or theoretical design. With using the inverse Lagrange transformation, (13) is discretized as follows: where f and h denote the nonlinear state equation function and observation equation function, respectively. w k and v k denote the process noise with a covariance matrix Q k and the observation noise with a covariance matrix R k , respectively. To define the mean and covariance of x k−1 arex k−1 and P k−1 respectively, for k ∈ {1, · · · , ∞}, the sigma vector set consisting of sampling points can be obtained as: where (n + λ)P k−1 i represents the column i of the matrix (n + λ)P k−1 , n = 4, i = {1, · · · , n}, and λ is an initial setting.
Using (14), the nonlinear transformation of sigma vectors in (15) is performed. Then, a new sigma vector set can be obtained as follows: By weighting the transformed sigma vectors from (16), the matrices of state and covariance for the first-step prediction can be given as: where χ j+1 k,k−1 is the column j + 1 of χ k,k−1 , j ∈ {0, · · · , 2n}, same as below. W (m) j and W (c) j denote the weights of mean and covariance, respectively.
To reduce the interference and improve the nonlinear estimation, the first-step predictions from (17) are subjected to the unscented transformation again, and then the secondgeneration sigma set is obtained: Then, the sigma vectors from (18) are substituted into the observation equation in (14) for nonlinear transformation, and the predicted observations are obtained: Besides, by weighted summations, the mean and covariance of the predicted observations are also obtained as follows: The covariance between the predicted state and the predicted observation can be calculated: Based on (20) and (21), the filter gain matrix is obtained: Finally, the system state and corresponding covariance in the UKF are estimated as: wherex is an initial constant compensation.
It follows (23) that the motion estimation error can be obtained as:θ whereθ U denotes the motion estimation from (23), and θ M denotes the actual measurement of the rotary encoder.
Generally, for each new system, the initial calibration with using sensors is an essential step in the precise control of the system. In order to compensate for the estimation error caused by neglecting the mass, damping, and friction of the cable in (12) and further approximate the system nonlinearity, it is necessary to model the estimation error for (24). Because the polynomial fitting is a standard and applicable method for nonlinear modeling [34], the polynomial fitting modeling is carried out by using the methods of Quasi-Newton and universal global optimization. In the cable-driven system, the actual rotation of the gripper jaw is closely related to the displacement of the driving and reset cables. Therefore, the measured motor angle and the displacement of optical encoder 2 are taken as the known variables to perform the model fitting, and then the error compensation model is obtained as follows: where {p 1 , p 2 , · · · , p 11 } are the fitting coefficients. Then, the error compensation model (25) is introduced into (23) to obtain a complete motion estimation model. Furthermore, the error model as a correction for the estimated state in each iteration of UKF can improve the prediction in the next iteration, and then the new estimated state is obtained as follows: where G = 1, 0, 0, 0 T .

IV. EXPERIMENTAL VERIFICATION A. EXPERIMENTAL SETUP
According to the model in Fig. 3(b), the experimental setup is built and illustrated in Fig. 5

B. EXPERIMENTS FOR THE UKF METHOD
According to the Fourier transform principle, it is generally known that the actual operation trajectory of surgical forceps can be approximated by a linear combination of trigonometric functions. Therefore, two sinusoidal trajectories are employed herein for motion estimation experiments In order to calibrate the system initially and obtain the error compensation model, the desired trajectory (27) is first used for motion estimation of the gripper jaw. The experimental results are shown in Fig. 6(a) and 6(b), and the errors of maximum, minimum, and root-mean-square (RMS) are approximately 0.27, −0.20, and 0.06 deg, respectively. After the error compensation model is determined, the estimation accuracy of the UKF method is experimentally verified by performing another desired trajectory (28).

1) First trajectory:
2) Second trajectory: where t denotes the system runtime.
As shown in Fig. 6(c) and 6(d), the estimated errors of maximum, minimum, and RMS are approximately 0.39, −0.44, and 0.12 deg, respectively. The cause resulting in more significant errors than that of the first trajectory is that the second trajectory requires the gripper jaw to change the rotation direction more frequently at a faster speed. However, the feasibility of the motion estimation model using the UKF method has been verified. Overall, the motion estimation errors of absolute and RMS are less than 0.5 and 0.2 deg, respectively. The position with significant error mainly occurs during the process of rapidly changing the rotation direction for the gripper jaw, which can be avoided in actual surgery due to stable and precise operations. In addition, the improvement of fabricating and assembly of mechanical parts can also contribute to the rapid response of the system.

C. COMPARISONS WITH THE LSMs
Among many motion estimation methods, the LSM is widely used because of its simplicity and strong practicality and can achieve satisfactory estimation performance [31]. Therefore, it is often used as a comparison object to evaluate the proposed method further. Here, comparisons of the proposed method with LSMs in motion estimation performance for cable-driven surgical forceps are conducted. The linear identification for the desired trajectory (27) of the gripper jaw is first carried out by using the conventional LSM. From (9), the identification model is expressed in a matrix as:  where θ s denotes the actual measurement of the gripper jaw. W s is an observation matrix, and χ s is an identification coefficient matrix.
Since the motion estimation using the LSM is a linear identification process, the nonlinear characteristics of the cabledriven system cannot be presented. After (29) is performed, the identification error is modeled as an approximation to compensate the system nonlinearity, and then the motion estimation model of compensated LSM can be obtained: whereθ e = (q 1 +q 3 ϕ m +q 5 x 2 +q 7 ϕ 2 m +q 9 x 2 2 +q 11 ϕ m x 2 ) (1+q 2 ϕ m +q 4 x 2 +q 6 ϕ 2 m +q 8 x 2 2 +q 10 ϕ 2 m x 2 ) is an error compensation model.
The experimental results of the second trajectory (28) are depicted in Fig. 7. Fig. 7(a) exhibits the estimation results using the LSM, with the errors of maximum, minimum, and RMS of 1.45, −1.08, and 0.57 deg, respectively. Moreover, the maximum, minimum, and RMS of estimation errors using the compensated LSM are 0.84, −0.79, and 0.26 deg, respectively, as shown in Fig. 7(b). These experimental results show that the error compensation model can be a valid approximation approach to compensate for the system's nonlinearity. The experimental results of three estimation methods are also presented in Table 2. It is evident that the estimation performance using the UKF method is better than that of the LSMs. Besides, the error amplitude is reduced by approximately 49% compared with the compensated LSM, and the RMS error is also improved by about 54%. Overall, these results show that the UKF method can significantly improve the accuracy of motion estimation for cable-driven surgical forceps.
Although the error compensation can help the LSM reduce the estimation error, it is just their simple arithmetic addition. After compensation, the results at each moment does not produce mutually beneficial effects. The error of the compensation model is the error of the compensated LSM. Differently, the error compensation model in the UKF algorithm can improve the estimated and predicted states, which are used in the next iteration. Furthermore, the centralized distribution of sampling points and the filter gains in each iteration can also be automatically optimized and updated to obtain new reliable state estimations. However, the nonlinear transformation accuracy of sigma vectors is partially weakened, restricting the estimation accuracy. Therefore, enriching the nonlinearity into the system state equation to improve the modeling accuracy will be extended in the future.
In addition, compared with the compensated LSM requiring dynamic information of two displacement sensors, only the displacement of reset cable is used in the UKF estimation model. Therefore, it can be concluded that the Strategy-2 of arranging the displacement sensor only on the reset cable is more practical for the motion estimation of cable-driven forceps. Furthermore, the Strategy-2 can reduce the load and friction on the motor side in the driving unit compared with the Strategy-3, which is desirable for mechanism design.

D. MOTION ESTIMATION WITH EXTERNAL FORCES ON THE GRIPPER JAW
In order to further verify the estimation accuracy of the UKF method in the condition that external force is applied on the tip of the gripper jaw, some force loading experiments are performed. As shown in Fig. 8, the gripper jaw is driven at a constant speed of 1.875 deg/s from the initial position θ = 0 deg to θ = 30 deg in each experiment. When the movement stops, the weights of 30, 50, 100, 200, and 250 g are manually loaded on the tip of the gripper jaw by using a fishing line, perpendicular to the tooth surface. During the period, the motion estimation of the gripper jaw is conducted by using the UKF method. Meanwhile, the actual rotation is    also recorded by the rotary encoder. The whole experimental process can be roughly divided into four phases. Phase I is a uniform motion without external forces on the gripper jaw. Phases II and IV are the motion stop phase before loading and the stable phase after loading, respectively. The experimental results are shown in Fig. 9, where different colors denote different cases, solid lines for measured results, and dotted lines for estimated results. The result curves for different loading weights are shifted appropriately in Fig. 9(a) for a clear distinction, which has been indicated in the legend. It is evident in Fig. 9(a) that the change in the rotation angle of the gripper jaw before and after loading is minimal. Moreover, phase III, with severe fluctuations, is a loading process.
As shown in Fig. 9(b), the error for phase III fluctuates the most seriously, which is mainly caused by the significant disturbance of manual loading. However, this phase is not the focus of the experiment. For phase I the errors of absolute and RMS are less than 0.5 and 0.2 deg, respectively. Moreover, the RMS error for phase II is also less than 0.2 deg. These experimental results have further validated the motion estimation performance of the UKF method. From phase IV, it can be concluded that the external force below 50 g hardly enforces the gripper jaw to rotate. Although the RMS error for phase IV is a little larger than that of phases I and II as the external force increases, it can still be no more than 0.5 deg.
The comparison among different phases is shown in Table 3. In practice, when the gripper jaw is loaded, the cables are forced to displace and elastically deform. Since the small elastic deformation is not accurately measured by the displacement sensor in the drive unit, a significant estimation error is caused. Therefore, an increase in cable tension will improve the motion estimation accuracy, even if an external force is applied to the gripper jaw.

V. CONCLUSION AND FUTURE WORK
In this paper, a motion estimation method based on UKF is presented to achieve high-precision motion estimation of cable-driven forceps in a robot-assisted surgical system. For this purpose, a cable-driven mechanism scheme is introduced, which is compact and practical to avoid cable looseness and mechanical clearance. By simplifying the flexible steel cable into a mass-spring-damper model, the system dynamics is modeled. After that, the motion estimation model based on UKF is obtained and validated by some experiments, with the results that the absolute and RMS estimation errors are less than 0.5 and 0.2 deg, respectively. By comparing with the LSMs, it can be concluded that the arrangement strategy of a displacement sensor only on the reset cable is practical. Besides, some estimation experiments in the case that external force is applied to the gripper jaw, are also performed. These experimental results have further verified that the UKF method can achieve high-precision motion estimation for cable-driven forceps without requiring sensors on the end joints or tool shaft of surgical instruments.
Future work includes the improvement of modeling accuracy for the cable-driven system and the motion estimation for dynamic loading. Furthermore, the closed-loop control and the indirect estimation of external forces for a 3-DOF surgical instrument based on the UKF method will also be extended. Since 2016, he has been a Research Fellow of the Department of Biomedical Engineering, National University of Singapore, Singapore. His research interests include medical robotics and devices, robotic system design, and mechanism analysis.