DYNAMIC COMPUTATION OF HAPTIC-ROBOT DEVICES FOR CONTROL OF A SURGICAL TRAINING SYSTEM

: This paper presents a new control model of the haptic device for the closed loop teleoperation of a minimal surgery training system. Dynamics of a 6-DOF parallel haptic device is computed and compensated to make a decoupled linearization control model. In teleoperation system, the master is the 6-DOF haptic device and the slave is the 6-DOF serial robot. The master haptic device provides the trajectories for the slave serial robot through the operation of user’s hand on the steering handle while the slave robot sends feedback forces on its end effector to the master controller in order to generate forces/moments on the steering handle of haptic master. In this manner the user’s hand will feel the forces/moments as the same those of the robot end effector. The feeling force tracking performances of system can be improved by using dynamic compensation and decoupled linearization controller based on fuzzy PID algorithms. Experiment results indicate that the dynamic compensation and fuzzy control can improve the control performances effectively.


INTRODUCTION
Robotics is emerging and attracting researchers [1][2][3]. Robots can be combined with haptic devices to make a master-slave system especially useful to handle remote objects in hostile environments or in a special environment such as minimal invasive surgery. Wireless networks have a lot of applications [4,5] and now they can be used to make teleoperation systems. The teleoperation in the virtual environment is also useful for training before it is tested in the real environment. In the closed loop teleoperation system, the user's hand will receive feedback forces on the master haptic device. However, the dynamics of the haptic device has strong influence that makes control performances reduce. In addition, because of highly nonlinear of haptic dynamics, it is difficult to effectively apply the linear control such as PID algorithms. Therefore, dynamics of haptic devices should be considered to compensate in the real time controllers.
There are two typical ways to derive dynamic equations of mechanisms such as Newton and Lagrange methods. Newton method [6][7][8] is developed based on the force and moment equations of all joints and links including reaction forces. Lagrange method [9][10][11][12] is developed based on conservation of energy. This method requires calculations of potential and kinetic energy. Lagrange method has simple calculation procedures so it is often selected to derive dynamic equations. The Lagrange method has two types [13] applied for two different structures. The first type can be used for open loop mechanisms such as serial robots whereas the second type is used for the closed loop mechanisms such as parallel robots. The inverse dynamic models can be used in the control actions to compensate for the real dynamic influence. The control performances can be improved by using fuzzy logic [15,16].
In this paper, inverse dynamics of a haptic device (master) is analyzed using Lagrange method to make a decoupled linearization model and construct a closed loop teleoperation control system. This analysis considers influences of all inertias, gravity and constraint torques. The gravity forces of links, frames and handle are calculated at their mass centers to obtain the accurate model. The modeling errors can be reduced by a control algorithm. PID control was selected for this haptic device in [14]. However, the experiments indicated that the tracking error performances can be improved if the PID parameters are tuned. Therefore, in this paper the dynamic compensation combined with decoupled linearization controllers based on self-tuning fuzzy PID algorithms is proposed to reduce force errors of haptic master device in the real time teleoperation system. The result of this paper is also the expansion of previous research [18].

A SURGICAL TRAINING SYSTEM
A minimal surgery training system is shown in Fig. 1. Where the master is a 6-DOF haptic device proposed by Vu and Na [14] replaces the 5-DOF Quanser haptic device [17] and a 6-DOF serial robot replaces Mitsubishi PA-10 robot. The haptic device consists of two 3-DOF translational parallel structures combined with a steering handle to provide 6-DOFs. The steering handle is designed to generate linear displacements when rotated. There are six motors attached on the base frame to support toques for active joints. The motors are selected with high torques so that the haptic device can provide big forces for the steering handle. Their weights do not affect to the operation of handle. These motors are attached with six high resolution encoders to measure rotation angles.
These are used to calculate kinematics and inverse dynamics of the 6-DOF haptic device. Links of the haptic device are made by hollow aluminum to reduce their weights. The links are connected by two 2-DOF revolute ball bearing joints to reduce frictions and extend rotation angles. The 6-DOF haptic device can provide a large workspace, low inertia and quick motions.
In teleoperation, the haptic device will track the trajectory of user hand. A user hand can keep a steering handle of haptic device to generate a movement as the desired trajectories for operation of the slave robot. However, there is a small position error between the user hand trajectory and the haptic device trajectory. This error causes a contact force between the user hand and haptic device. This contact force can be measured by two 3-DOF force sensors attached on the steering handle of haptic device. In the closed loop teleoperation, the contacting forces of the slave robot end effector on the environment (mass-damper system) will be measured by a 6-DOF force sensor as the desired forces/moments for the controller of the haptic device. Vu and Na [14] described the relationship between the 6-DOF force/torque sensor on the slave robot and two 3-DOF force sensors on the haptic master.

DYNAMIC COMPUTATION
The coordinate systems and kinematic analysis are presented by Vu [14]. This section will extend that work for inverse dynamic analysis and compensation.

COORDINATE SYSTEMS
Coordinate systems of the 6-DOF haptic device are described in Fig. 2 Fig. 2 (a) 6-DOF haptic structure.
(b) The first leg.

POSITION VECTORS
Three types of coordinate systems have the relationship together through transformation matrixes (rotation and translation matrixes). The rotation matrix (1) The translation matrix i D from the local coordinate system to the upper and lower coordinate systems is also expressed as (4) Position vectors are firstly calculated on the local coordinate systems and then they are transformed to the global coordinate systems though transformation matrixes. Let i C 1 and i C 2 be the mass center of the first and second links. Position vectors of mass centers and end of links ( Position vectors of upper and lower end effectors are then calculated as The position vectors considered on the local coordinate system should be transformed to the upper and lower coordinate systems through the rotation matrix i T and translational matrix i D . And then they are transformed to the global coordinate system through the rotation matrix u T , b T and The position vector of the main end effectors (center of steering handle) can be calculated on the global coordinate system as:

VELOCITY VECTORS
All six legs of haptic device have the same structure. Therefore only velocities of leg 1 should be calculated in details so that the general case is obtained in the same manner. The link 1 of leg 1 rotates about 1 1 w O so its angular velocity on the local coordinate system is 1 11 11 This angular velocity can be transformed to the global coordinate system through the rotation matrix 1 R R u as The linear velocity of M1 on the local coordinate system is calculated as Some sub-coordinate systems should be added on the leg 1 to calculate the velocity of link 2. Firstly attach a sub-coordinate system

Relationship between
as shown in Fig. 3 Finally the frame of The angular velocities 11   , 21   are described in the frame of Fig. 3(d). The angular velocity of frame 31   21  11  21  31  21  11 cos , , sin Therefore the angular velocity of link M1P1 is calculated on the local coordinate system as This angular velocity is reorganized as The linear velocity of mass center of link 2 on the global coordinate system is calculated as The linear velocity of P1 on the global coordinate system is also calculated as

CONSTRAINT FUNCTIONS
The haptic device consists of six legs connected with two moving frames and a steering handle. Each leg has a 3-DOF serial structure and has kinematic constraints with the moving frame. Each leg has three equations which express the relationship between joint angles of leg and position of moving frames along three axes. These equations are also used to solve forward kinematics in [14]. Therefore constraint functions can be selected based leg equations as where  

LAGRANGE FOMULATION
The potential energy of haptic device includes the mass influences of all links, two moving frames and a steering handle is calculated as where 1i C and 2i C be the mass center of the first and second links of legs; O is the origin of coordinate system; m1i and m2i are mass of the first and second links; mh, mu and mb are mass of steering handle, upper and lower moving frames, respectively.
The kinetic energy includes moving influences of six legs, two moving frames and a steering handle. The total kinetic energy of haptic device is calculated as Lagrange function is defined as Let Mij be the (i, j) element of matrix M so the Lagrange function is rewritten as; Therefore, the dynamic equation of 6-DOF haptic device has the following form where M is the inertia torque vector, V is a velocity coupling torque, G a gravitational torque, H is a constraint torque and f is the friction vector. Once  are inertia matrix, coupling velocity matrix, and gravity force of the haptic device respectively; is position vector of the steering handle.

CONTROL PERFORMANCES
The teleoperation system using haptic devices has been described in the previous works [14]. The teleoperation in this part is the expansion of research [18]. A diagram is constructed as shown in Fig. 4. where the inverse dynamic model, h M is added to reduce dynamic influences. The teleoperation control system can be divided into two main parts: position control of slave serial robot and force control of master haptic device. Users can generate motions of hand so the haptic device can follow with motion h x . The error between two motions creates the contact force c F . The motions of the haptic device are measured by encoders. These joint angles are used to calculate forward kinematics of haptic device so that its trajectory is determined. This trajectory is composed of three positions and three rotations of the center of the handle. The slave serial robot uses this trajectory to find joint angle commands, θrc through inverse kinematics Gr. The error between the command, θrc and feedback, θr joint angles is the input of position controller, Kr(s). Its output is converted into voltage commands, Vr for the robot's motors. The rotations of robot joints determine the trajectory of end effector, xr. There exist some tracking errors because of robot dynamics. When the slave serial robot is manipulated, its end effector may touch the environment Ze. The contacting force, Fd (or Fe), is mechanically transmitted to the joints of robot through its transpose Jacobian matrix T r J . This force works as the external disturbances so it may reduce tracking performances of the robot.
When the user force c F is applied on the center of the handle, it is converted to the forces 12 F at the upper and lower end effectors by the transformation matrix 1 L  . The forces 12 F are measured by two 3-DOF force sensors. The user force c F is also mechanically transmitted to the joint torque u  by the Jacobian matrix of h J . The user manipulating the haptic device should feel actual contact force from the environment not those of the structure of the haptic device. Detecting the actual contact force from environment by using a haptic device may present a hardware design problem such as low inertia and frictions.
For some commercial haptic devices the open loop force control can be applied. The contact force d F from environment is directly converted to the motor forces m  . The user then may feel the contact forces as well as undesired haptic dynamic forces due to inertia, joint frictions, and gravity.

LINEARIZATION MODEL
If the dynamics of the haptic device is compensated, a linearization dynamic model can be defined to develop control systems as shown in the Fig. 5  The impedances along , ,    directions are defined as The relationship between contact force c F and position errors X  between user hand and haptic device is expressed as The user hand impedance along each direction is modeled as a spring k and damper b [19][20][21] as shown in Fig. 6. These parameters are increased if the position error is bigger. When the position errors approach to an upper limitation, the parameters of user hand are sharply increased. These are unknown parameters and difficult to get accurate values. However their influences can be compensated by a force controller.
The above explaination is very inportant to develop the control simulation on Matlab software.
Models of haptic device and user hand are assumed as decoupled components so each direction of force vector can be controlled independently by a 1-DOF force controller.

FUZZY PID CONTROL
Linear controllers can be applied with the decoupled linearization dynamic model.
A PID controller was selected for Kh(s) in [14] where the experiments indicated that this controller could reduce influence of gravity, inertia and friction. The PID parameters should be adjusted to smaller values due to the dynamic compensation. Experiments with PID controllers also indicate that if Kp, Ki, Kd parameters are tuned, the tracking error performances can be improved. Moreover the 6-DOF haptic device has the following aspects: i.
A user's hand has high nonlinear dynamics so it is difficult to acquire a precise model. ii.
Gravities of structures affect to force performances even though they are attempted to minimize in design. iii.
Frictions of motor shafts and joints do influences on the force performances. iv.
Noises from force sensors also disturb force responses. Therefore, a self-tuning fuzzy PID controller can be selected for Kh(s) to control forces. The structure of this controller is shown in Fig. 7. The output surface of fuzzy controller should be considered as the main results of design produce. They depend on the membership functions and rules. Three different controllers with 9, 25 and 49 rules are chosen as shown in Fig. 8 and their output responses with respect to the first input if the second input equals zeros are shown in Fig. 9. The results indicate that the higher number of rules can produce more linear responses. However, if there are so many rules, the sampling time of the real digital controller is very big. This makes difficulty in the real control system. Therefore, the limitation of rules is selected as 49.
Symmetric Gaussian functions are selected for both input and output membership functions of fuzzy controllers as      Parameters σ, c are selected to find the membership functions. The 7 linguistic terms of input variables are defined as shown in the Table 1 Table 2.

EXPERIMENT RESULTS
The hardware of teleoperation system consists of two digital controllers as dSPACE1103 and dSPACE1104. These controllers include almost useful interfaces such as ADC, DAC, Encoder, I/O and RS232, RS485 so they are very convenient to connect with a real system. In addition, these controllers are supported to work directly with SIMULINK/MATLAB in real time so the heavy control algorithms also can be implemented to do experiments. These controllers can communicate with computers by ControlDesk software to monitor and record data. The contact environment is constructed as a spring and damper system to supply contacting forces for the end effector of serial robot. This contacting force is measured by a 6-DOF force sensor whose signals are transmitted to dSPACE1103 controller to process.
Firstly, the step responses of three different fuzzy controllers with 9, 25 and 49 rules are implemented for the comparison. The desired forces Fd are selected as 5N and the desired torques of zeros. The measured user force Fc of on the haptic device and Fd are shown in Fig. 10-12.   The responses indicate that the fuzzy controller with 49 rules provide better performances. Therefore, this controller is selected for the next experiments.
Secondly, the control of teleoperation system is implemented for free motions. This is means that the haptic device is mechanically manipulated with the desired forces/torque of zeros. The measured user force Fc of free motions is considered to be purely the dynamic forces caused by the haptic device. Shaking motions of the steering handle were generated for the closed loop force control of free motions. Very much the shaking motions of the steering handle were also generated for the force control of free motions. The trajectories of the steering handle are shown in Fig. 13. The contact forces Fc on the haptic device were measured and compared to zeros as shown in Fig. 14. This figure shows that the amplitude of the contact force for this control is reduced by much more compared to that for the PID control [14]. Finally, experiment results of the closed loop teleoperation system with dynamic compensation and fuzzy control are recorded when the user keeps the haptic handle to move the serial robot whose end effector contacts the environments such as a massdamper system.
The contacting forces exerted on the robot end effector are measured by a 6-DOF force sensor while the forces on the haptic handle are measured by two 3-DOF force sensors. These comparisons are shown in Fig. 15. indicate that the tracking force performances in this proposed control system could be well improved compared to the PID controller shown in [16], as well. The trajectories of these force control are also shown in Fig. 16.

CONCLUSION
This is the first time the dynamic model of a 6-DOF parallel haptic device is derived and compensated on the real time controller for the minimal surgery training system. A new model of haptic device and user hand is also introduced based on the linearization. The new model of haptic device is described based on the real haptic device system where the user hand drives handle of haptic device and trajectory errors between user hand and haptic device generates contact forces.
Force tracking performances of a minimal surgery training system using the 6-DOF haptic and robot devices have improved by implementing dynamic compensation combined with fuzzy PID control. The inverse dynamic model of the 6-DOF haptic device is calculated base on the second type of Lagrange method. Influences of all inertia, gravity, constraint and frictions are considered. The mass centers of links are considered at their real locations so the modeling error may be reduced. Calculated joint torques of six active joints is used for the compensation control in the teleoperation system.
Three fuzzy controllers with 9, 25 and 49 rules are designed and implemented on the digital controller dSPACE 1103 indecate the higher rule controller produce better responses.
The tracking performances of the measured user forces on the haptic device and desired zeros contact forces for the free movement are compared. When the dynamic compensation combined with the fuzzy control is applied, some force errors between the desired zeros forces and the user hand forces could be significantly reduced. Moreover in the teleoperation experiment, the user applied forces of the steering handle could track the contact forces from environment well.