Control Performance Evaluation of Serial Urology Manipulator by Virtual Prototyping

Prostatic hyperplasia and tumor are common diseases, and the minimally invasive surgery inserting the instruments through the urethra into the prostate is commonly conducted. Taking the robotic manipulator for such surgery into consideration, this paper analyses the workspace of the end effector, and proposes the distribution error of the fixed point and the tracking error of manipulator end effector on the cone bottom surface of the workspace as the basis for control implementation of the manipulator. The D-H coordinate system of the manipulator is established and the trajectory planning of the end effector in the Cartesian space is carried out. The digital model was established, and dynamics simulation was performed in Solidworks and Matlab/Simulink environment to guide the manipulator design. Trajectory mapping and synchronization control between virtual model and the actual manipulator are realized based on digital twin technique. The virtual manipulator can reflect the real-time state of the manipulator with data interaction by comparing the dynamics simulation results with the motor current values obtained by experiment. Experiment was carried out with PD feedback control and Newton–Euler dynamics based feedforward control to get the trajectory tracking characteristic of each motor, errors of the fixed point and tracking performance of the end effector of the manipulator. The results show that compared with PD feedback control, feed forward control implementation can achieve a reduction of 30.0% in the average error of the fixed point of the manipulator and a reduction of 33.3% in the maximum error.


Introduction
Hyperplasia and tumor of urinary organ are common and frequently-occurring diseases in clinical medicine. With the population aging in China, demand for urinary surgery such as laser ablation of prostatic hyperplasia is increasing. According to statistics, prostate hyperplasia surgery accounts for more than 1/3 of the total urological surgery. Because of the large number of cases and the hard working of the operation, it is critical for doctors to carry out high-intensity and precise surgery continuously.
In recent years, robot-assisted medicine developed rapidly, and the combination of minimally invasive surgery and intelligent robotics is a popular research topic. Many kinds of surgical robots have made great progress, and the minimally invasive surgery robot which can deliver the instruments into patient body through body cavity such as urethra has become a hotspot [1]. Taking the advantages of accurate and stable performance, the application of robotic manipulator into minimally invasive surgery can not only eliminate the influence of doctors' hand tremor to improve the quality of the operation, but also reduce the difficulty of operation and the workload of doctors [2].
In minimally invasive surgery, the end effector of the instrument always rotates around the insertion point of the human body as a vertex to reduce the damage to the tissue. The point can also be called fixed point or remote center. The rotation of surgical instruments around the fixed point can be realized through multiple joints control of serial manipulators, mechanical configuration constraints, and passive joints usually [3]. As a leading surgical robot, Da Vinci robot uses mechanical configuration constraints at the end effector and has been successfully applied to a variety of surgeries. However, its large volume and high cost have restricted its application scope [4]. Zeus manipulator uses passive joints and it performed remote control surgery experiment, and studied the delay of operation control and operation effect [5,6]. Li et al. [7,8] proposed an integrated method of spatial RCM (Remote Center of Motion) mechanism, which was successfully applied in China's first master-slave minimally invasive surgical robot MicroHand A, and studied the system real-time interactions and master-slave delay experimentally. Locke et al. [9] used a serial manipulator to control the manipulator rotation around the fixed point and studied the precision of the manipulator end effector and optimized motion parameters by software algorithm. Basically, the robotic manipulator is a complicated mechatronic system. The physical prototype needs to be adjusted and improved repeatedly in the traditional design process. However, the operations are complicated and time-consuming. Establishing a virtual prototype by using a computer software environment before building a physical prototype can assist in system improvement and testing, effectively shorten the development cycle and reducing cost [10,11]. Manipulator dynamics simulation can be used to predict the movement and joint toque of the manipulator in various operating conditions, providing a basis for the design of the manipulator, the size of the link, and the selection of the driving motor [12,13]. Chen et al. [14] used Solidworks to model the surgical manipulator and used ADAMS software to simulate and analyze the dynamic performance of the manipulator after simplifying the components, then obtained the position error of the end effector. Kang et al. [15] used Simulink SimMechanics to perform dynamic simulation of a serial manipulator, and studied the relationship between the movement and driving torque of each joint. Realizing the synchronous control between the virtual manipulator and the actual manipulator requires communication between the computer and the manipulator. The real-time control is an essential prerequisite which requires a control system with high frequency. Miro surgical robot form a distributed control system on six PCs. The control software is developed in MATLAB/Simulink and runs on the QNX system with 100 Hz control frequency [16]. The SPRINT robot adopts the mode of upper computer and lower computer. The development and code generated is completed in Simulink. The sample frequency of the upper computer reaches 1000 Hz [17].
On the other hand, the manipulator is a non-linear mechanical system and it is difficult to achieve good control results with conventional control. It is necessary to model the dynamics of the manipulator and design the manipulator controller. Mi et al. [18] used Lagrange method to establish the dynamic equation of the manipulator, and designed a sliding model controller to achieve the trajectory tracking of the manipulator. Su [19] built the dynamics model of the manipulator and designed an adaptively compensated controller to study the trajectory tracking accuracy changes experimentally. Chen et al. [20] modeled the dynamics of intraocular surgical manipulator and used gravity compensated PD controllers to make the manipulator had better tracking characteristics.
In order to overcome the shortcomings of the existing surgical robots when used in urological surgery, this paper proposes a serial surgical manipulator. Evaluating the accuracy and control effect of the manipulator by the distribution error of the fixed point and the tracking error of the manipulator end effector on the cone bottom surface of the workspace. The kinematics model of the manipulator was established and the dynamic simulation of the manipulator joints was performed using a virtual prototype, and the trajectory mapping and synchronization control between the virtual manipulator model and the physical manipulator were realized. PD feedback control was employed in experiments to obtain the trajectory tracking curve of each motor, the distribution error of the fixed point and the tracking error of the end effector. Newton-Euler dynamic equation was used to perform feedforward control by transforming the manipulator into an approximately linear system, showing better control performance experiments.

Motions of Urologic Surgical Manipulator
For the laser ablation of prostatic hyperplasia in minimally invasive surgery, the movement and workspace of surgical instruments were analyzed to ensure that the designed surgical manipulator could meet the clinical operation requirements. The surgical instrument takes the insertion point of the patient urethra as the fulcrum and the end effector is seen as constrained by the insertion point. The location of the insertion point is approximately fixed, so the insertion point is called the fixed point or remote center. Because of the constraint by this point, the end effector loses two degrees of freedom. The manipulator is used to move the end effector to the diseased part of the tissue and a laser generator is installed at the end of the instrument to perform laser excision. The end effector has only four degrees of freedom: two rotations around the fixed point, namely pitch and yaw, rotation around the axis of the end effector and translation along the axis. In addition, two rotations around the fixed point and the movement along the axis ensure that the end effector can reach any point in its workspace. The rotation around the axis ensure the adjustment of the end effector's orientation. The above degrees of freedom enable the workspace of the end effector to be a cone, which takes the fixed point as the vertex, as shown in Figure 1.
To achieve the movement around a fixed point, mechanical constraints, passive joints, and serial manipulator can be used [21,22]. The first two methods require complex mechanical designs and are expensive, the serial manipulator achieves target movement by kinematics and control algorithms, which has the advantages of low cost, high rigidity and short development cycle [23]. So this research focuses on the serial manipulator. The distribution error of the fixed point and the tracking error of the end effector to the trajectory of the underside of the cone are analyzed. The motion parameters of the end effector were obtained from the clinical experience of the surgeon. The maximum rotation angle of the end effector around the fixed point of the urethra is 45° and the maximum insertion length is 200 mm. The conical workspace is described in the form of polar coordinates, which can be expressed by the rotation angle α around the axis and the rotation angle β around the vertex.
The D-H method is a commonly used to describe the kinematic relationship of the robotic mechanism by coordinate transformation. In this research, the modified D-H coordinate system of the manipulator is established, as shown in Figure 2. The origin of coordinate system x 6 y 6 z 6 is the fixed point of the manipulator, and the origin of coordinate system x 7 y 7 z 7 is the laser generator installation point at the end effector. The two coordinate systems are located at different parts of the same link.
The position and orientation of the fixed point on the end effector can be expressed by the homogeneous transformation matrix: where n x , n y , n z is the direction vector of the x-axis of the end effector's work coordinate system in the base coordinate system, and o x , o y , o z is the direction vector of the (1) y-axis of the end effector's work coordinate system in the base coordinate system, and a x , a y , a z is the direction vector of the z-axis of the end effector's work coordinate system in the base coordinate system, and p x , p y , p z is the location vector of the origin of the end effector's work coordinate system in the base coordinate system. During the surgery operation, the manipulator and the patient are static. p x , p y , p z is the fixed point of the end effector, and the coordinates of fixed point is a constant in the base coordinate system. In Figure 1, the z-axis is the orientation of end effector. By setting the polar coordinates, a x , a y , a z is given by Given p x = 200, p y = 0, p z = 100, the position and orientation of the end effector can be obtained. Considering link parameters of manipulator d 1 = 0, a 2 = 153.4, d 4 = 190, d 6 = 50, the inverse kinematics of manipulator is solved [24]. The angle of each joint corresponding to the target position and orientation is obtained as the control variable of the joint motors.
In order to prevent the vibration of the manipulator caused by the sudden change of driving torque during the movement of the manipulator, the trajectory planning is adopted to make the manipulator move smoothly. The Cartesian space of the end effector is obtained by quantic polynomial method, and then the real-time inverse kinematics is calculated to formulate the joint space of the manipulator. As for the start self-test motion of the manipulator, it is the process of rotating the end effector around the edge of the cone workspace and measuring the motion accuracy. To get the trajectory planning in Cartesian space, assuming β= 45 • and the start selftest motion is completed in 10 s, and spline expression is given by In order to make movement steady, α(0) = 0, α(10) = 360,α(0) = 0,α(10) = 0,α(0) = 0,α(10) = 0, so It will be used as the trajectory planning function of the manipulator in the Cartesian space later.

Virtual Prototyping Based Simulation
Robot simulation can show the response of mechanical structure by using real-time data in virtual model and to test and optimize the manipulator in virtual model. Software in the computer is used to establish the virtual  manipulator, and kinetic parameters are added to the virtual manipulator. Virtual prototyping can reduce the experimental cost and develop the system control algorithm more quickly [25]. Combined with dynamics simulation software, an accurate virtual model of manipulator can be established in 3D modeling software. In Solidworks software, the serial manipulator is designed for urological surgery and simplified. The model is then imported into Matlab/Simulink to establish the dynamic model. The dynamic model includes the world coordinate system, the setting of the gravitational field, the six revolute joints of the manipulator and its input and output ports, the coordinate transformation information and the dynamic parameters of the six links of the manipulator. Relevant parameters can be read in Solidworks to ensure the precision of dynamics simulation. The first picture in Figure 3 is the structural model in Solidworks software, the second picture is the simplified virtual manipulator running diagram in Simulink and it shows the movement of the manipulator and the third is the actual manipulator.
In the dynamics simulation of the manipulator, the trajectory planning in the Cartesian space is added, and bushing joint in Multibody was used to perform the inverse dynamics simulation of the manipulator, then obtain the driving torque of each joint of the manipulator. Joint 6 performs only self-rotating around the axis, which does not affect the overall orientation and position of the end effector, so it is not considered later. The driving torques of the rest five joints obtained in  Figure 4. For each joint simulation torque in Figure 4, the torques exerted by joint 2 and joint 3 are circumferential forces caused by gravity. The joints need to generate enough torques to offset the circumferential force and drive the whole manipulator to move, so these joints torque are large. Joint 5 is also affected by the circumferential force caused by gravity, but it is located at the end of the manipulator. It makes that the power arm is short and the gravity is small, so the torque on joint 5 is smaller. Joint 4 mainly receives axial force and radial force and the circumferential force that affects the torque is small. It can be seen from the installation position of the joint 1 that the axial force received by the joint 1 is borne by the bearing, and the radial and circumferential forces are zero, so the joint 1 only needs a small torque to offset the bearing friction and the inertia of the manipulator to achieve the slow motion. The manipulator driving torque can be used as the guide of motor selection during the design of manipulator. Once the driving motor was selected by the simulation results, and the relevant dynamic parameters of the newly selected motor are updated into the model above for re-simulation, and then the design is iterated. Finally, the manipulator design meets the requirements. According to the driving torque of the manipulator and the torque-speed characteristic curve of the motor, Dynamixel MX106T is chosen in the joint 2 and joint 3, Dynamixel MX64T is chosen in joint 5, and Dynamixel MX28T is chosen in the joint 1, joint 4 and joint 6.
By means of trajectory mapping and synchronous control between the virtual model and the real manipulator, they can maintain the same operating characteristics, namely digital twin technique. It was used in many applications such as robot teaching, factory automation simulation, etc. [26]. The real-time control of the motor is realized in Simulink, combined with the dynamic model of virtual manipulator operates in the meantime, the algorithm can be validated and applied to the real manipulator.
The system mainly includes main control unit, motion execution unit, drive unit, sensors and input/ output device. Input device is a master manipulator, its movement parameters are collected by encoder and a STM32F407 MCU read the control values and send them to PC. This research uses the motion of system start self-test by the trajectory planning to study the motion performance of the manipulator. The main control unit is finished in Simulink, including Cartesian coordinate planning, inverse kinematics calculation, control algorithm, sending control data to motors and reading sensors feedback data. The motor of each joint is Dynamixel MX series digital steering engine, and it includes control chip, driver, sensor, motor and other components. The S-Function is established in Simulink to control the digital motor. The control function is realized by C Mex mode, which was used for embedded systems with high control rate. Initializing the input and output ports, data type, dimension and system sample time of the motor control function, adding the firmware library and header files of the motor, and using C language to call motor's API to send control commands and read sensor data. Finally, generate mexw64 executable file to improving the efficiency of the system [27]. The control model as shown in Figure 5 summarizes the above process and it can realize the synchronous control between virtual manipulator and real manipulator, and the control frequency can reach 300 Hz by experiment.
Using the above model, the synchronous control between the virtual manipulator and the real manipulator is realized, as shown in Figure 6. The virtual manipulator in Simulink and the actual manipulator have the same motion.
Using the model in Figure 5, the driving current of each motor in the real manipulator is obtained, as shown in Figure 7. Compared with the simulation results shown in Figure 4, it can be found that the current value and the simulation torque value have the same change trend. Motor 2 and motor 3 have bigger current values than others and it is the same as the simulation torques in Figure 4. Virtual manipulator by Simulink can partly reflect the real-time state of the real manipulator. This model can also realize real-time control algorithm, and eventually deliver it to real manipulator, which improves the efficiency of control algorithm design.

Experiment by PD Controller
On the basis of the simulation based control system obtained in the previous section, PD controller is employed to analyze the trajectory tracking of the manipulator. The system block diagram of PD controller is shown in Figure 8. The trajectory tracking curve and following error of motor 1 to motor 5 with PD controller are shown in Figure 9. The target angles of motors are obtained by the inverse kinematics in the Cartesian space after trajectory planning. The following error of each motor is given by It can be obtained that the average following error from motor 1 to motor 5 is 0.0027, 0.0067, 0.0082, 0.0072 and 0.0032 radian. Motor 2 and motor 3 are mainly interfered by gravity, and the error is relatively large.
During the whole movement process of the manipulator by PD controller, the distribution error of the fixed point on the end effector is shown in Figure 10.
The mean value of the position error of the fixed point is given by It can be obtained that the mean value of the position error of the fixed point is 4.0 mm, and maximum error is 6.3 mm.
During the whole movement process of the manipulator by PD controller, the trajectory tracking curve of the end effector is shown in Figure 11.
The mean value of following error of the end effector position is given by It can be obtained that the mean value of the following error of the end effector position is 5.9 mm, and maximum error is 9.7 mm.

Establishment of Dynamic Equation
There exists relatively large error in PD control implementation of the manipulator. The error mainly comes from the nonlinearity of the manipulator. The manipulator is a time-varying nonlinear system, and the joints exert influence on each other. The synchronization control between the virtual manipulator and the real manipulator has been realized before, but it is difficult to achieve the precise control for motors. It is necessary to consider the influence of dynamic parameters and establish dynamic equations for manipulator control system. The methods of dynamics equation establishment of manipulator include Newton-Euler method and Lagrange method [28]. The former calculates forces and torques between links by force balance recursively, while the latter describes the dynamic equation by kinetic energy and potential energy. For the serial manipulator with six degrees of freedom, Newton-Euler method has a higher computation efficiency [29]. In this section, the dynamic equation is established by using iterative Newton-Euler method. The coordinate transformation of the manipulator has been taken into consideration. The rotation coordinate transformations of each link are i i+1 R , which are given by (8)  The dynamic equation involves the length of the link, the position of the center of mass of link and the inertial tensor. The position vector of the origin of the next coordinate system with respect to this coordinate system is represented with i p i+1 , and it is a constant equal to the length of each link, as shown below: The position of the center of mass and the inertial tensor are generally difficult to obtain in practice. However, they can be obtained from the mass attribute in Solidworks software and used for dynamic modeling of the manipulator.
Newton-Euler equation is formulized based on the force balance between links, and it is composed of two parts: outward iterations and inward iterations. The outward iterations are a set of formulas from the base coordinate system to the end effector tool coordinate system, and the forces and torques on the center of mass of each link are calculated from the given manipulator joint angular position, joint angular velocity and angular acceleration. The outward iteration formulas of the Newton-Euler equation are as follows:  where i is 0 to 5, so there are six groups of outward iteration formulas of the manipulator. The first parameters used in above formulas are where 0 ω 0 and 0ω 0 are the angular velocity and angular acceleration of the base coordinates of the manipulator. Because the base coordinate system is fixed, they (11) are taken as 0. 0v 0 is the linear acceleration of the base coordinate system. Considering the effect of gravity, the manipulator is placed in a coordinate system rising with the acceleration of gravity g . The acceleration opposite to the acceleration of gravity is added to replace the force brought by the gravity of the mechanical system structure.
After obtaining the six groups of outward iteration formulas of the manipulator, the inward iterations method takes effect. They are a set of formulas from the end effector tool coordinate system to the base coordinate system. The force and torque applied to the joint are calculated from the given force and torque acting on the center of mass of each link. The inward iteration formulas of Newton-Euler equation are as follows: where i is 6 to 1, so there are six groups of inward iterations formulas of the manipulator, and i+1 i R is the inverse matrix of i i+1 R in outward iterations formulas. The first parameters used in above formulas are: where 7 f 7 is the force exerted on the end effector, and 7 n 7 is the torque on the end effector.
During the operation, the movement of the end effector around the fixed point should be strictly satisfied to prevent excessive contact pressure between the manipulator and the human body. The smaller the contact stress, the better the patient feels. So they are taken as 0 ideally.
The above iterative Newton-Euler dynamic formulation is then programmed in Simulink. It also can be expressed as where τ is the vector of the driving torques of each motor, M(�) is the mass matrix of the system, and it is only related to the joint angle of the manipulator. ¨ is angular acceleration vector of the manipulator joint. V (�,�) is centrifugal force and Coriolis force vector. G(�) is the gravity vector. The formula is also called as the statespace equation of manipulator, and it is a nonlinear timevarying system.

Experiment by Feedforward Controller
In the process of establishing the dynamic equation of the manipulator, the friction force is not considered because of complexity and difficulty. With the feedforward control method, the state space equation of the manipulator is added into the control system, and the system is linearly approximated. The block diagram of the control system is shown in Figure 12. The coefficient in feedforward controller is consistent with that presented in PD control.
In the operating environment of the manipulator, the moving speed is low, so the gravity is significant. The trajectory tracking curve and following error of motor 1 to motor 5 in feedforward controller are shown in Figure 13. It can be seen that the average following error from motor 1 to motor 5 is 0.0038, 0.0033, 0.0043, 0.0088 and 0.0033, respectively. Compared with PD control, the errors of motor 2 and motor 3, which are greatly affected by gravity, are significantly reduced. The mean error of motor 2 was reduced from 0.0067 to 0.0033, with a reduction of 50.74%. The mean error of motor 3 was reduced from 0.0082 to 0.0043, with a reduction of 47.56%. The error variations of the other three motors are small.
The error distribution of fixed point on the end effector by feedforward control is shown in Figure 14. Compared with PD control, the mean error of fixed point is 2.8 mm, with a reduction of 30.0%. The maximum error is 4.2 mm, with a reduction of 33.3%.
The trajectory tracking curve of the end effector is shown in Figure 15. Compared with PD control, the mean error of the trajectory tracking is 2.7 mm, with a reduction of 54.2%. The maximum error is 6.6 mm, with a reduction of 32.0%.

Conclusions
For the laser ablation of prostatic hyperplasia in urology, the trajectory planning of serial manipulator was used to control the end effector to fit the surgical instrument workspace. With Solidworks and Simulink to establish a virtual manipulator, the dynamics simulation was performed, and the trajectory mapping and synchronization control between the virtual manipulator and the actual manipulator was finished. The virtual manipulator can reflect the working condition of the actual manipulator real-timely. The model was developed with control algorithm and it lays the foundation for subsequent experiments.
Using PD feedback control, the average following errors from motor 1 to 5 are 0.0027, 0.0067, 0.0082, 0.0072, 0.0032 radians, respectively. The average error of the fixed point on the manipulator is 4.0 mm, and the maximum error is 6.3 mm. The average error of the trajectory tracking of the end effector is 5.9 mm, and the maximum error is 9.7 mm.
The Newton-Euler method is used to establish the dynamic model of the manipulator, and the approximate dynamic model is used in feedforward control, so the manipulator system is linearly approximated. The gravity term is dominant at low speed. The average following errors from motor 1 to motor 5 are 0.0038, 0.0033, 0.0043, 0.0088, 0.0033 radians, respectively. The error of motor 2 and motor 3, which are greatly affected by gravity, is significantly reduced. The average error of motor 2 is reduced from 0.0067 to 0.0033, with