Skip to main content

Control Performance Evaluation of Serial Urology Manipulator by Virtual Prototyping

Abstract

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.

1 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.

2 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.

Figure 1
figure 1

DOFs and workspace of the end effector

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 x6y6z6 is the fixed point of the manipulator, and the origin of coordinate system x7y7z7 is the laser generator installation point at the end effector. The two coordinate systems are located at different parts of the same link.

Figure 2
figure 2

Modified DH coordinate system of serial manipulator

The position and orientation of the fixed point on the end effector can be expressed by the homogeneous transformation matrix:

$$_{6}^{0} T = \left[ {\begin{array}{*{20}c} {n_{x} } & {o_{x} } & {a_{x} } & {p_{x} } \\ {n_{y} } & {o_{y} } & {a_{y} } & {p_{y} } \\ {n_{z} } & {o_{z} } & {a_{z} } & {p_{z} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right],$$
(1)

where nx, ny, nz is the direction vector of the x-axis of the end effector’s work coordinate system in the base coordinate system, and ox, oy, oz is the direction vector of the y-axis of the end effector’s work coordinate system in the base coordinate system, and ax, ay, az is the direction vector of the z-axis of the end effector’s work coordinate system in the base coordinate system, and px, py, pz 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. px, py, pz 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, ax, ay, az is given by

$$\left\{ {\begin{array}{*{20}l} {a_{x} = \cos \beta ,} \hfill \\ {a_{y} = - \sin \beta \sin \alpha ,} \hfill \\ {a_{z} = \sin \beta \cos \alpha .} \hfill \\ \end{array} } \right.$$
(2)

Given px = 200, py = 0, pz = 100, the position and orientation of the end effector can be obtained. Considering link parameters of manipulator d1 = 0, a2 = 153.4, d4 = 190, d6 = 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 \(\beta { = 45}^\circ\) and the start self-test motion is completed in 10 s, and spline expression is given by

$$\alpha (t) = a_{5} t^{5} + a_{4} t^{4} + a_{3} t^{3} + a_{2} t^{2} + a_{1} t + a_{0} .$$
(3)

In order to make movement steady, \(\alpha (0) = 0,\)\(\alpha (10) = 360,\;\dot{\alpha }(0) = 0,\;\dot{\alpha }(10) = 0,\;\ddot{\alpha }(0) = 0,\;\ddot{\alpha }(10) = 0,\) so

$$\alpha (t) = 0.0216t^{5} - 0.54t^{4} + 3.6t^{3} .$$
(4)

It will be used as the trajectory planning function of the manipulator in the Cartesian space later.

3 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.

Figure 3
figure 3

3D model, simulation model and 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 simulation are shown 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.

Figure 4
figure 4

Driving torque of each joint in simulation

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.

Figure 5
figure 5

Block diagram of simulation control system

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.

Figure 6
figure 6

Synchronous control system between the virtual manipulator and the real manipulator

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.

Figure 7
figure 7

Measured driving current of each motor

4 Control Implementation and Dynamics Modeling

4.1 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.

Figure 8
figure 8

Block diagram of PD control system

The trajectory tracking curve and following error of motor 1 to motor 5 with PD controller are shown in Figure 9.

Figure 9
figure 9

Trajectory tracking curve and following error of each motor with PD controller

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

$$\overline{e} = \frac{{\sum\nolimits_{i = 1}^{n} {\left| {\theta_{{i_{goal} }} - \theta_{{i_{actual} }} } \right|} }}{n}.$$
(5)

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.

Figure 10
figure 10

Error distribution of the fixed point by PD control

The mean value of the position error of the fixed point is given by

$$\overline{r} = \frac{{\sum\nolimits_{i = 1}^{n} {\sqrt {y_{i}^{2} + z_{i}^{2} } } }}{n}.$$
(6)

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.

Figure 11
figure 11

Tracking trajectory of the end-effector

The mean value of following error of the end effector position is given by

$$\overline{r} = \frac{{\sum\nolimits_{i = 1}^{n} {\sqrt {(y_{{i_{{{\text{goal}}}} }} - y_{{i_{actual} }} )^{2} + (z_{{i_{{{\text{goal}}}} }} - z_{{i_{actual} }} )^{2} } } }}{n}.$$
(7)

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.

4.2 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 + 1}^{i} R\), which are given by

$$\begin{gathered}_{1}^{0} R = \left[ {\begin{array}{*{20}c} {c_{1} } & { - s_{1} } & 0 \\ {s_{1} } & {c_{1} } & 0 \\ 0 & 0 & 1 \\ \end{array} } \right],\quad_{2}^{1} R = \left[ {\begin{array}{*{20}c} {c_{2} } & { - s_{2} } & 0 \\ 0 & 0 & { - 1} \\ {s_{2} } & {c_{2} } & 0 \\ \end{array} } \right], \hfill \\_{3}^{2} R = \left[ {\begin{array}{*{20}c} {c_{3} } & { - s_{3} } & 0 \\ {s_{3} } & {c_{3} } & 0 \\ 0 & 0 & 1 \\ \end{array} } \right],\quad_{4}^{3} R = \left[ {\begin{array}{*{20}c} {c_{4} } & { - s_{4} } & 0 \\ 0 & 0 & { - 1} \\ {s_{4} } & {c_{4} } & 0 \\ \end{array} } \right], \hfill \\_{5}^{4} R = \left[ {\begin{array}{*{20}c} {c_{5} } & { - s_{5} } & 0 \\ 0 & 0 & 1 \\ { - s_{5} } & { - c_{5} } & 0 \\ \end{array} } \right],\quad_{6}^{5} R = \left[ {\begin{array}{*{20}c} {c_{6} } & { - s_{6} } & 0 \\ 0 & 0 & { - 1} \\ {s_{6} } & {c_{6} } & 0 \\ \end{array} } \right], \hfill \\_{7}^{6} R = \left[ {\begin{array}{*{20}c} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{array} } \right]. \hfill \\ \end{gathered}$$
(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:

$$\begin{gathered} {}^{0}p_{1} = [0,\;0,\;0]^{{\text{T}}} ,{}^{1}p_{2} = [0,\;0,\;d_{1} ]^{{\text{T}}} ,{}^{2}p_{3} = [a_{2} ,\;0,\;0]^{{\text{T}}} ,{}^{3}p_{4} = [0,\; - d_{{4_{1} }} ,\;0]^{{\text{T}}} , \hfill \\ {}^{4}p_{5} = [0,\;0,\;d_{{4_{2} }} ]^{{\text{T}}} ,{}^{5}p_{6} = [0,\; - d_{6} ,\;0]^{{\text{T}}} , \cdots {}^{6}p_{7} = [0,\;0,\;d_{7} ]^{{\text{T}}} . \hfill \\ \end{gathered}$$
(9)

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:

$$\left\{ {\begin{array}{*{20}l} {{}^{i + 1}\omega_{i + 1} = {}_{i}^{i + 1} R{}^{i}\omega_{i} + \dot{\theta }_{i + 1} {}^{i + 1}\hat{Z}_{i + 1} ,} \hfill \\ {{}^{i + 1}\dot{\omega }_{i + 1} = {}_{i}^{i + 1} R{}^{i}\dot{\omega }_{i} + {}_{i}^{i + 1} R{}^{i}\omega_{i} \times \dot{\theta }_{i + 1} {}^{i + 1}\hat{Z}_{i + 1} + \ddot{\theta }_{i + 1} {}^{i + 1}\hat{Z}_{i + 1} ,} \hfill \\ {{}^{i + 1}\dot{v}_{i + 1} = {}_{i}^{i + 1} R({}^{i}\dot{\omega }_{i} \times {}^{i}p_{i + 1} + {}^{i}\omega_{i} \times ({}^{i}\omega_{i} \times {}^{i}p_{i + 1} ) + {}^{i}\dot{v}_{i} ),} \hfill \\ {{}^{i + 1}\dot{v}_{{C_{i + 1} }} = {}^{i + 1}\dot{\omega }_{i + 1} \times {}^{i + 1}p_{{C_{i + 1} }} + {}^{i + 1}\omega_{i + 1} \times ({}^{i + 1}\dot{\omega }_{i + 1} \times {}^{i + 1}p_{{C_{i + 1} }} ) + {}^{i + 1}\dot{v}_{i + 1} ,} \hfill \\ {{}^{i + 1}F_{i + 1} = m_{i + 1} {}^{i + 1}\dot{v}_{{C_{i + 1} }} ,} \hfill \\ {{}^{i + 1}N_{i + 1} = {}^{{C_{i + 1} }}I_{i + 1} {}^{i + 1}\dot{\omega }_{i + 1} + {}^{i + 1}\omega_{i + 1} \times {}^{{C_{i + 1} }}I_{i + 1} {}^{i + 1}\omega_{i + 1} ,} \hfill \\ \end{array} } \right.$$
(10)

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

$${}^{0}\omega_{0} = [0,\;0,\;0]^{{\text{T}}} ,{}^{0}\dot{\omega }_{0} = [0,\;0,\;0]^{{\text{T}}} ,{}^{0}\dot{v}_{0} = [0,\;0,\;g]^{{\text{T}}} ,$$
(11)

where \({}^{0}\omega_{0}\) and \({}^{0}\dot{\omega }_{0}\) are the angular velocity and angular acceleration of the base coordinates of the manipulator. Because the base coordinate system is fixed, they are taken as 0. \({}^{0}\dot{v}_{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:

$$\left\{ {\begin{array}{*{20}l} {{}^{i}f_{i} = {}_{i + 1}^{i} R{}^{i + 1}f_{i + 1} + {}^{i}F_{i} ,} \hfill \\ {{}^{i}n_{i} = {}^{i}N_{i} + {}_{i + 1}^{i} R{}^{i + 1}n_{i + 1} + {}^{i}p_{{C_{i} }} \times {}^{i}F_{i} + {}^{i}p_{i + 1} \times {}_{i + 1}^{i} R{}^{i + 1}f_{i + 1} ,} \hfill \\ {\tau_{i} = {}^{i}n_{i}^{T} {}^{i}\hat{Z}_{i} ,} \hfill \\ \end{array} } \right.$$
(12)

where i is 6 to 1, so there are six groups of inward iterations formulas of the manipulator, and \({}_{i}^{i + 1} R\) is the inverse matrix of \({}_{i + 1}^{i} R\) in outward iterations formulas. The first parameters used in above formulas are:

$${}^{7}f_{7} = [0,\;0,\;0]^{{\text{T}}} , \space\space\space\space\space {}^{7}n_{7} = [0,\;0,\;0]^{{\text{T}}} ,$$
(13)

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

$$\tau = M(\Theta )\ddot{\Theta } + V(\Theta ,\dot{\Theta }) + G(\Theta ),$$
(14)

where \(\tau\) is the vector of the driving torques of each motor, \(M(\Theta )\) is the mass matrix of the system, and it is only related to the joint angle of the manipulator. \(\ddot{\Theta }\) is angular acceleration vector of the manipulator joint. \(V(\Theta ,\dot{\Theta })\) is centrifugal force and Coriolis force vector. \(G(\Theta )\) is the gravity vector. The formula is also called as the state-space equation of manipulator, and it is a nonlinear time-varying system.

4.3 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.

Figure 12
figure 12

Block diagram of feedforward control system

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.

Figure 13
figure 13

Trajectory tracking curve and following error of each motor in feedforward controller

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%.

Figure 14
figure 14

Error distribution of the fixed point by feed forward control

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%.

Figure 15
figure 15

Tracking trajectory of the end-effector

5 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 a reduction of 50.74%; and the average error of motor 3 was reduced from 0.0082 to 0.0043, with a reduction of 47.56%. The average error of the fixed point on the end effector was 2.8 mm, with a reduction of 30.0%; and the maximum error was 4.2 mm, with a reduction of 33.3%. The average trajectory tracking error of the end effector is 2.7 mm, with a reduction of 54.2%, and the maximum error is 6.6 mm, with a reduction of 32.0%. So feedforward control has a better control effect. The laser ablation of prostatic hyperplasia is a classic operation, the accuracy requirement is relatively low. During the operation, the doctor’s experience is required to control the manipulator. In the case of the master-slave operation by the doctor, the accuracy of the manipulator can meet the needs of the operation.

References

  1. Z F Li, Y Yang, Y Su, et al. Current status and thinking of research and application of surgical robots in China. China Medical Equipment, 2019, 16(11): 177-181.

    Article  Google Scholar 

  2. O Meireles, S Horgan. Applications of surgical robotics in general surgery. Surgical Robotics: Systems Applications and Visions, 2011: 791-812.

    Book  Google Scholar 

  3. M Feng, Y L Fu, B Pan, et al. Design and implementation of the robotic end effector for minimally invasive celiac surgery. Robot, 2009, 31(1): 47-52.

    Google Scholar 

  4. X M Du, Y S Zhang. Advances in application of Da Vinci surgical system. China Medical Equipment, 2011, 8(5): 60–63.

    Google Scholar 

  5. J Marescaux, J Leroy, F Rubino, et al. Transcontinental robot-assisted remote telesurgery: Feasibility and potential applications. Annals of Surgery, 2002, 253(4): 487–492.

    Article  Google Scholar 

  6. J Marescaux, J Leroy, M Gagner, et al. Transatlantic robot-assisted telesurgery. Nature, 2001, 413: 379-380.

    Article  Google Scholar 

  7. J M Li, S X Wang, X F Wang, et al. Optimization of a novel mechanism for a minimally invasive surgery robot. The International Journal of Medical Robotics and Computer Assisted Surgery, 2010, 6(1): 83–90.

    Google Scholar 

  8. J M Li, N X Zhou, S X Wang, et al. Design of an integrated master-slave robotic system for minimally invasive surgery. The International Journal of Medical Robotics and Computer Assisted Surgery, 2012, 8(1): 77-84.

    Article  Google Scholar 

  9. R C Locke, R V Patel. Optimal remote center-of-motion location for robotics-assisted minimally invasive surgery. IEEE ICRA, Roma, 2007: 1900-1905.

    Google Scholar 

  10. R Q Ma, S H Hao, W F Zheng, et al. Research on coordinated simulation of robot arm based on MATLAB and ADAMS. Machinery Design & Manufacture, 2010, 4: 93–95.

    Google Scholar 

  11. G Y Wu, Y S Liu, Z Niu, et al. Research on the dead-point problem of spherical micro pump based on virtual prototype. IEEE International Conference on Aircraft Utility Systems, IEEE, 2016.

  12. H Q Sang, C He, J M Li, et al. Dynamic modeling and trajectory tracking control for a 3-dof instrument in minimally invasive surgery. IEEE Robotics and Biomimetics (ROBIO), 2010.

  13. F Edris, S G Liu. An efficient approach for manipulator robot dynamics modeling. International Conference on Computer, Control, Electrical, and Electronics Engineering (ICCCEEE), 2018.

  14. J P Chen, H X Chen, X L Yang, et al. Dynamic simulation analysis of six-degree of freedom medical robotic couch for radiation therapy. China Medical Devices, 2019, 34(8): 76-80.

    Google Scholar 

  15. X Y Kang, Y X Zhao, X Chen. Simulation research of 6-DOF robot arm based on simmechanics. Machine Tool & Hydraulics, 2016, 44(23): 23–29.

    Google Scholar 

  16. U Hagn, R Konietschke, A Tobergte, et al. DLR MiroSurge: A versatile system for research in endoscopic telesurgery. International Journal of Computer Assisted Radiology & Surgery, 2010, 5(2): 183–193.

    Article  Google Scholar 

  17. M Niccolini, G Petroni, A Menciassi, et al. Real-time control architecture of a novel Single-Port lapaRoscopy bimaNual roboT (SPRINT). IEEE International Conference on Robotics and Automation, Roma, 2012: 3395-3400.

  18. G S Mi, H Q Liang. Nonlinear sliding mode control for robotic manipulator based on disturbance observer. Journal of System Simulation, 2019, 31(9): 1935-1941.

    Google Scholar 

  19. W Y Su. Research on trajectory tracking adaptive control algorithm for industrial manipulators. Nanjing: Southeast University, 2018.

    Google Scholar 

  20. X Chen, L Huang, Y Yang. Gravity compensation of an intraocular surgery robot based on computed torque method. Journal of Beijing University of Aeronautics and Astronautics, 2017, 43(6): 1231-1238.

    Google Scholar 

  21. C Y He, L Huang, Y Yang, et al. Research and realization of a master-slave robotic system for retinal vascular bypass surgery. Chinese Journal of Mechanical Engineering, 2018, 31:78.

    Article  Google Scholar 

  22. S Aksungur, M Aydin, O Yakut. Real-time PID control of a novel RCM mechanism designed and manufactured for use in laparoscopic surgery. Industrial Robot, 2019, 47(2): 153-166.

    Article  Google Scholar 

  23. Y L Fu, B Pan. Research progress of surgical robot for minimally invasive robot. Journal of Harbin Institute of Technology, 2019, 51(1): 1-11.

    Google Scholar 

  24. J J Li, H Shi, L Yang. Inverse kinematics analysis, size optimization and error analysis of urology surgical manipulator. Biomedical Engineering International Conference, 2018.

  25. Z Z Wang, C J Yang, C Y Liu, et al. United simulation of 6 DOF robot with MATLAB and ADAMS. Manufacturing Automation, 2013, 18: 30-33.

    Google Scholar 

  26. H Li, Z X Ma, Z S Ma. Integrated system and modularization of industrial robot. Beijing: Chemical Industry Press, 2018.

    Google Scholar 

  27. Hyowinner. Simulink simulation and code generation technology. Beijing: Beijing University of Aeronautics and Astronautics Press, 2015.

  28. M J Thomas, M L Joy, A P Sudheer. Kinematic and dynamic analysis of a 3-PRUS spatial parallel manipulator. Chinese Journal of Mechanical Engineering, 2020, 33:13.

    Article  Google Scholar 

  29. W He, W Ge, Y Li, et al. Model identification and control design for a humanoid robot. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 2017, 47(1): 45-57.

    Article  Google Scholar 

Download references

Acknowledgements

The authors sincerely thanks to Doctor Lin Yang in the first affiliated hospital of Xi’an Jiaotong University for his critical discussion and kind guide during kinematic modeling of surgical instrument.

Funding

Supported by National Natural Science Foundation of China (Grant No. 51675400).

Author information

Authors and Affiliations

Authors

Contributions

HS was in charge of proposing the conception and 3D modeling; JL wrote the draft manuscript and conducted simulation; LG assisted with experiment and data analysis; XM checked and improved the manuscript in writing. All authors read and approved the final manuscript.

Authors’ Information

Hu Shi, born in 1983, is currently an associate professor at School of Mechanical Engineering, Xi'an Jiaotong University, China. He received his doctor degree from Zhejiang University, China, in 2012. His research interests include hydraulic system and intelligent robotics. Tel: +86-13095710086.

Jiajie Li, born in 1995, is currently a master candidate at School of Mechanical Engineering, Xi'an Jiaotong University, China.

Lianjie Guo, born in 1996, is currently a master candidate at School of Mechanical Engineering, Xi'an Jiaotong University, China.

Xuesong Mei, is currently a professor at School of Mechanical Engineering, Xi'an Jiaotong University, China. His research interests include advanced manufacture and intelligent robotics.

Corresponding author

Correspondence to Xuesong Mei.

Ethics declarations

Competing Interests

The authors declare no competing financial interests.

Rights and permissions

Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Shi, H., Li, J., Guo, L. et al. Control Performance Evaluation of Serial Urology Manipulator by Virtual Prototyping. Chin. J. Mech. Eng. 34, 25 (2021). https://doi.org/10.1186/s10033-021-00534-x

Download citation

  • Received:

  • Revised:

  • Accepted:

  • Published:

  • DOI: https://doi.org/10.1186/s10033-021-00534-x

Keywords