# Establishing the Improved Dynamic Model for the Extracorporeal Ultrasonic Lithotripsy Medical Cooperative Robot

Authors: Xiao Li; Maocai Li; Hanxu Sun; Jun Gong; Zhaowen Zhang; Rongshun Zhang
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

The calculus is one of the common diseases with high incidence. The effective treatment method is extracorporeal ultrasonic lithotripsy. At present, it is low about the intelligent and automatic level of the lithotripter, and it has gradually failed to meet the treatment needs. The extracorporeal ultrasonic lithotripsy medical cooperative robot can solve such problems effectively, and it is equally critical for accurate modeling studies of dynamic models. Based on the previous research and experimental basis, this paper proposes a correction theory to improve the accuracy of the dynamic model for the model error in collaborative robot work. The study first establishes the dynamic model and the solid model of the collaborative robot and then subtracts the value of the dynamic model from the solid model to obtain the modified equation. Finally, the accuracy of the dynamic model is improved by modifying the equation. The experiments show that the kinetic model correction theory is effective and can improve the accuracy of the dynamic model modeling after the correction of the torque equation. The experiments show that the improved dynamic model theory is effective and can improve the modeling accuracy of the dynamic model after the correction of the torque equation. The modified equation has the best correction effect in the 5th degree polynomial and can be used for the extracorporeal ultrasonic lithotripsy medical cooperative robot control.


---

## Body

## 1. Introduction

In recent years, the research on medical collaborative robot has been in the ascendant and has gradually achieved good applications in the medical field. Zhang et al. summarized the characteristics of the development of the medical robot industry, analyzed the characteristics of 63 mainstream medical robot enterprises, and comprehensively combed the overall pattern of medical robot development [1]. This provides a reference for what kind of robotic arm is used in this paper. Therefore, the robot used in this paper is a collaborative robot.Mihelj et al detailed the characteristics of collaborative robot and explained the differences between them and traditional industrial robot in terms of protection, working conditions, and working environment. They explained that the characteristics of collaborative robot work together with people [2]. Ren et al. studied the method of collaborative robot teaching and learning [3]. This provides an idea for cooperative robot data acquisition in this paper. Faccio et al. studied the comparison model of collaboration and traditional robot assembly and compared the differences between manual assembly and noncooperative automatic assembly [4], which provided a reference for researching cooperative robot to change probes. Zanchettin et al. pointed out that the flexibility and finiteness of robot hindered the use of robot by small and medium-sized companies in the collaborative robot special report. Collaborative robot can successfully solve this kind of problem [5], which indirectly supports the research of medical collaborative robot. Gennaro et al studied the impedance control of cooperative robot in safe operation and proposed a new cooperative robot impedance control technology. The controller realizes safe human-machine cooperation through energy and power limitation and verifies the impedance using KUKA LWR 4+ arm [6]. The control algorithm provides an idea for the obstacle avoidance and collision algorithm of the debris robot and also provides a research direction for high-precision dynamic modeling.Xiao G et al studied a new cooperative multirobot path planning algorithm, which adopts a multirobot system architecture combining centralized and distributed. The problem of global path planning and local path planning is solved by the fusion immune coevolution algorithm and artificial potential field method, and the adaptive level of the robot is improved [7]. This provides a method reference for the experimental path planning of the extracorporeal ultrasonic lithotripsy robot. Mao et al studied the safety control of collaborative robot on the detection platform. Through software design, the man-machine cooperation in the work area was realized to avoid accidents and has high safety [8], which provided a reference for the safety design of the debris robot. Yan et al. studied real-time estimation of single-point contact information of all-mechanical arm based on momentum observation and optimization algorithm in man-machine cooperation and proposed a kind of information based on robot control and motion state. Based on the momentum observation and optimization algorithm, the real-time estimation method of the single-point contact information of the whole manipulator realizes the external force perception of the mechanical arm without the aid of the sensor [9]. This provides a new idea for the collision algorithm and path planning of the extracorporeal ultrasonic lithotripsy robot. Chen studied the cooperative robot adaptive strategy of double Gaussian process, which enables the robot to adapt to the pose change of the target object and obtain smooth joint motion [10], which provides a reference method for the human-machine collaborative experiment of the debris robot. Xiao studied the collision detection algorithm for lightweight modular cooperative robot and proposed a collision detection algorithm without adding additional sensors and changing the complexity of the control system [11]. This provides a new theoretical reference for dynamic collision detection of the extracorporeal ultrasonic lithotripsy robot. He J studied the impedance adaptive control system of the variable-load dual-arm robot and proposed a double-loop impedance variable stiffness tracking control strategy. The method of decomposing the resultant force into internal force and external force is used to control the inner loop impedance and the outer loop impedance, respectively. It solved the problem of stability control of two-arm cooperative grasping and object-to-environment exchange force [12], which provides a reference method for dynamic modeling and control of the extracorporeal ultrasonic lithotripsy robot.For the kinematics and dynamics of the medical collaborative robot, common kinematics description methods include D-H parameter method, improved D-H parameter method, and exponential product equation method (POE) [13–15]. The common methods for dynamics modeling include the Lagrange method [16, 17], Newton–Euler method [18–20], Kane method [21–24], spin-to-even method [25, 26], and Virtual work principle method [27, 28]. The Lagrange method has a large amount of calculation when the robot has a high degree of freedom, but avoids the calculation of linear acceleration and angular acceleration. The Newton–Euler method is too complicated in multidegree of freedom, but the calculation is comprehensive, so the influence of inertia is often simplified or ignored. The Kane method avoids derivation and has high computational efficiency. It is also used in dynamic modeling and high-precision control of a parallel robot [29–31], which is convenient for computer control. It considers the sum of generalized dynamics and generalized inertial forces to be zero [27, 32, 33]. Zheng et al modeling analysis, reliability, and response of nonlinear dynamics in terms of multilink joints and friction [34–37] provided references for the modeling of this paper.For the content of this paper, the solid model is more accurate than the traditional dynamic model, but it cannot be directly used in the control system. Traditional dynamic models are less accurate but can be used directly in control systems. The improved dynamic model is established by experimental data of the solid model and the traditional dynamic model. The accuracy of the dynamic model is improved by the correction equation. Mathematical modeling and experimental methods are used to solve the undetermined coefficient matrix of the modified equation under the task. It is proved that the complement correction equation is 5th-order linear correlation in the extracorporeal ultrasonic lithotripsy medical cooperative robot.


## 2. The Composition and Parameters of the Cooperative Robot

The medical cooperative robot system is mainly composed of three parts: the treatment bed, the control system, and the cooperative arm system. The Figure1 shows the design of the bed and the dual-cooperative manipulator system. The dynamic modeling study of this paper is carried out around the 6-DOF collaborative robot system that is shown in Figure 2. Figures 2(a)–2(c) are front, side, and rear views, respectively, of the cooperative robot.Figure 1
Design sketch of the medical cooperative robot system.Figure 2
The medical cooperative robot experimental prototype.
(a)
(b)
(c)During treatment, the doctor determines the location of the stone and sends a position signal to the control system. The cooperating robot then receives the position signal and plans the path for the robotic actuator to actively reach the gravel position and begin the lithotripsy treatment. Its dynamic response requires high speed and accuracy. This research uses a collaborative medical robot system developed by the laboratory, as shown in Figure2. It has six single degree-of-freedom joint. No. 1 model is used for the bottom-up and one-two joint. No. 2 model is used for the three-four joint. No. 3 model is used for the five-six joint. And the parameters of the cooperative robot and joint are shown in Table 1. The experimental and solid modeling of this paper is based on these parameters.Table 1
The parameters of robot and joint.
Designation
Unit
No. 1
No. 2
No. 3
Rated torque
Nm
480
180
100
Repeated accuracy
degrees
≤0.01
≤0.01
≤0.01
Peak torque
Nm
1200
700
340
Weight
kg
12.8
7.6
5.6
Rotation angle
degrees
≤360
≤360
≤360
Reduction ratio
1
160
160
120
Maximum angular velocity
rpm
34
74
74
Nominal voltage
VDC
48
48
48
Payload
13 kg
Total weight
55 kg
Working radius
1400 mm
Repeat positioning accuracy
±0.02 mm
Degree of freedom
6
TCP maximum speed
1.6 m/s
Ambient temperature
−10°C∼50°C
Electrical interface
USB; RS232; CAN-bus
Power supply
230 V AC 50–60 Hz
Power consumption
Typical operating conditions are approximately 650 W


## 3. Establish the Kinematic Model

According to Figure2, the schematic diagram of the robot is shown in Figure 3. In the figure, Z1∼Z6 are the z-axes of the Cartesian coordinate system of each joint. Since the lithotripsy mechanism of the end effector is rigid, it is omitted in the kinetic study to replace the part with a rigid body.Figure 3
The collaborative robotic mechanism diagram.According to the size relationship and coordinate system position, the D-H parameters of the collaborative robot are listed in Table2. Furthermore, the kinematic transformation matrix is obtained.Table 2
The D-H parameters of the cooperative robot.
Linki
α
i
−
1
a
i
−
1 (mm)
d
i
θ
i
1
0
0
420
0
2
Pi/2
0
0
Pi/2
3
Pi
254
0
Pi/2
4
Pi/2
0
469
0
5
Pi/2
0
111
0
6
-Pi/2
0
251
0Then, the kinematic model equation is shown as follows:(1)T=A1⋅A2⋅A3⋅A4⋅A5⋅A6=nxoxnyoyaxpxaypynzoz00azpz01.


## 4. Establish the Dynamic Model

The Kane method is a method for establishing dynamic equations using the D’Alembert principle and the virtual displacement principle. It is a general method to establish the dynamic model of robot mechanism. The basic idea is to replace generalized coordinates as the independent variable of the system. It means that the sum of generalized dynamics and generalized inertial forces is equal to zero. The outstanding advantage of the Kane equation method in dynamic modeling is that it only needs to calculate the vector dot product and cross product operation, which avoids the derivative operation. Therefore, the number of calculations in the Kane method dynamics modeling process is much less than the other methods, greatly improving the efficiency of the calculation and facilitating control.According to the calculation process of the Kane method, the initial conditions of the operation are as follows.According to the parameters of the solid model, the rotation matrix of each joint is shown as follows:(2)R01=cθ1−sθ10sθ1cθ10001,R12=cθ2−sθ2000−1sθ2cθ20,R23=cθ3−sθ30−sθ3−cθ3000−1,R34=cθ4−sθ4000−1sθ4cθ40,R45=cθ5−sθ5000−1sθ5cθ50,R56=cθ6−sθ60001−sθ6−cθ60.The position vector of the centroidCi of the i member described by the rod coordinate system i, whose position vector is as follows:(3)r1r2r3r4r5r6=0127000000−58.46058.300−1001470−8500.The inertia tensor matrix of each joint in the coordinate system is as shown in equations (4a)–(4f). The unit is kg⋅mm2:(4a)I1=Ixx1Ixy1Ixz1Iyx1Iyy1Iyz1Izx1Izy1Izz1=313.65000313.6500048.61,(4b)I2=Ixx2Ixy2Ixz2Iyx2Iyy2Iyz2Izx2Izy2Izz2=348.550−258.580781.470−258.580482.17,(4c)I3=Ixx3Ixy3Ixz3Iyx3Iyy3Iyz3Izx3Izy3Izz3=132.8800030.39000132.88,(4d)I4=Ixx4Ixy4Ixz4Iyx4Iyy4Iyz4Izx4Izy4Izz4=95.0100095.0100012.11,(4e)I5=Ixx5Ixy5Ixz5Iyx5Iyy5Iyz5Izx5Izy5Izz5=49.370008.3400049.37,(4f)I6=Ixx6Ixy6Ixz6Iyx6Iyy6Iyz6Izx6Izy6Izz6=2.060002.050002.05.According to the kinematic equation and structure size, the generalized displacement is inferred as follows:(5)P1˜=00420T,P2˜=000T,P3˜=0−2540T,P4˜=00469T,P5˜=00111T,P6˜=00251T.The Kane method needs to derive the generalized velocity and acceleration values from joint 1 to joint 6. For the rotating joint, it used the following equation to calculate. The generalized speed is as shown in equation (6). The generalized velocity of the centroid is shown in equations (7) and (8). The general angular velocity is as shown in equation (9). The generalized angular velocity is obtained, as shown in equation (10). Generalized angular acceleration is shown in equation (11). Generalized acceleration is shown in equation (12). The generalized acceleration of the centroid is as shown in equation (13):(6)v˜i=ω˜i×P˜i+Ri−1iv˜i−1,(7)v˜ci=v˜i+ω˜i×Ri,(8)v˜ci,θ˙j=dv˜cidθ˙j,(9)ω˜i=Ri−1iω˜i−1+θ˙ie0,(10)ω˜i,θ˙j=dω˜idθ˙j,(11)ω˙˜i=Ri−1iω˙˜i−1+ωi−1×θ˙ie0+θ¨ie0,(12)v˙˜i=Ri−1iv˙˜i−1+ω˙˜i×P˜i+ω˜i×ω˜i×P˜i,(13)v˙˜ci=v˙˜i+ω˙˜i×P˜i+ω˜i×ω˜i×P˜i.The initial conditions determined by the working conditions are shown in equation (14). It assumes that the Z0-axis of the absolute reference frame is perpendicular to the ground and points upwards:(14)ω0=0,ω˙0=0,v0=0,v˙0=00gT.The Kane method is a dynamic method based on the Kane dynamic equation. The main dynamic equations are shown in equations (15) and (16):(15)N˜n=Inω˙˜n+ω˜i×In⋅ω˜iT,(16)τθj=∑i=jnmiv˙˜ciT⋅vci,θ˙j+∑i=jnN˜iω˜i,θ˙j.No additional external force load is applied at the end as a simplified condition. Substituting the known parameters into equations (15) and (16), the final torque expression can be obtained as equations (17a)–(17f) after iterative calculation:(17a)τ1=m1v˙˜c1T⋅vc1,θ˙1+m2v˙˜c2T⋅vc2,θ˙1+m3v˙˜c3T⋅vc3,θ˙1+m4v˙˜c4T⋅vc4,θ˙1+m5v˙˜c5Tvc5,θ˙1+m6v˙˜c6T⋅vc6,θ˙1+N˜1ω˜1,θ˙1+N˜2ω˜2,θ˙1+N˜3ω˜3,θ˙1+N˜4ω˜4,θ˙1+N˜5ω˜5,θ˙1+N˜6ω˜6,θ˙1,(17b)τ2=m2v˙˜c2T⋅vc2,θ˙2+m3v˙˜c3T⋅vc3,θ˙2+m4v˙˜c4T⋅vc4,θ˙2+m5v˙˜c5T⋅vc5,θ˙2+m6v˙˜c6T⋅vc6,θ˙2+N˜2ω˜2,θ˙2+N˜3ω˜3,θ˙2+N˜4ω˜4,θ˙2+N˜5ω˜5,θ˙2+N˜6ω˜6,θ˙2,(17c)τ3=m3v˙˜c3T⋅vc3,θ˙3+m4v˙˜c4T⋅vc4,θ˙3+m5v˙˜c5T⋅vc5,θ˙3+m6v˙˜c6T⋅vc6,θ˙3+N˜3ω˜3,θ˙3+N˜4ω˜4,θ˙3+N˜5ω˜5,θ˙3+N˜6ω˜6,θ˙3,(17d)τ4=m4v˙˜c4T⋅vc4,θ˙4+m5v˙˜c5T⋅vc5,θ˙4+m6v˙˜c6T⋅vc6,θ˙4+N˜4ω˜4,θ˙4+N˜4ω˜4,θ˙4+N˜5ω˜5,θ˙4+N˜6ω˜6,θ˙4,(17e)τ5=m5v˙˜c5T⋅vc5,θ˙5+m6v˙˜c6T⋅vc6,θ˙5+N˜5ω˜5,θ˙5+N˜6ω˜6,θ˙5,(17f)τ6=m6v˙˜c6T⋅vc6,θ˙6+N˜6ω˜6,θ˙6.In equations (17a)–(17f), N˜n=Inω˙˜n+ω˜i×In⋅ω˜iT.After the final torque expression is collated, the total expression of the dynamics model can be obtained as follows:(18)τj=∑i=jnmiv˙˜ciT⋅vci,θ˙j+∑i=jnInω˙˜n+ω˜i×In⋅ω˜iTω˜i,θ˙j,j=1∼6.


## 5. Dynamic Model Analysis

Path planning is performed on the extracorporeal ultrasonic lithotripsy medical cooperative robot, and the result is substituted into equations (17a)–(17f). The main parameters of the substitution are shown in Table 3. The motion displacement curve of the end effector is shown in Figure 4.Table 3
The parameters of the dynamic model.
Designation
Unit
Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6
Generalized displacement
x
mm
0
0
0
0
0
0
y
mm
0
0
−254
0
0
0
z
mm
420
0
0
469
111
251
Mass center
x
mm
0
127
0
0
0
0
y
mm
0
0
−58.46
0
58.30
0
z
mm
−100
147
0
−85
0
0
Joint quality
kg
12.8
12.8
7.6
7.6
5.6
5.6
Inertia tensor
kg
⋅
mm
2
Equation (4a)
Equation (4b)
Equation (4c)
Equation (4d)
Equation (4e)
Equation (4f)
Initial conditions
ω
0
=
0, ω˙0=0, v0=0, v˙o=00gT, e0=001TFigure 4
End effector motion displacement curve.The conditions are input into the dynamic model, and the response characteristics of the dynamic model under the task plan are shown in Figure5.Figure 5
Response curve of each joint dynamics model.Then, based on the dynamic model parameters and the collaborative robot shown in Figure2, the ADAMS solid model is established. Its parameters are exactly the same as the collaborative robot. Substituting the input conditions shown in Figure 4 into the solid model, the dynamic response curves of the joints in the solid model are shown in Figure 6. Motion 1 to motion 6 in Figure 6 represents the response curves of joint 1 to joint 6.Figure 6
Response curve of each joint solid model. Now, we can analyze the error of the dynamic model.


## 6. Improved Dynamic Model Analysis and Modeling

Based on dynamic modeling and solid modeling, the model error is analyzed in this part. First of all, there is an error in all dynamic models, which is the difference between the actual torque variation and the torque variation in the mathematical model. That is, the error is equal to the actual value minus the mathematical model value. The error is defined asε, as shown in equation (19), where τ is the actual moment function of the dynamic model and τt is the amount of moment change in the mathematical model.When the accuracy of the dynamic model can be improved, it is considered that the correction functionτ′ exists so that equation (20) is established. The ε′ is defined as the error value after the kinetic model improves the accuracy, as shown in equation (20):(19)ε=τ−τt,(20)ε′=τ−τt+τ′.The expression of the correction functionτ′ derived from equations (19) and (20) is shown in equation (21), where Δε is the amount of change in the model error:(21)τ′=τ−τt−ε′=ε−ε′=Δε.As we all know, the real value of the dynamic model is often difficult to find and the excessively complex modeling equation makes the model lose the possibility of practical application. When the model is constructed to obtain an error lower than the general mathematical model, we believe that it is closer to the true value, and instead of the actual value for modeling and calculation, better modeling results can still be obtained. In the study of the correction functionτ′, we use the actual value τt+τ′ instead of the real value τ for modeling and calculation. This is because we are more concerned with the improvement of the accuracy of the model itself, rather than the model directly reaching the true value.The correction functionτ′ is related to the time parameter t. According to Taylor’s theorem, τ′ is an nth-order function of time t. The correction function is defined, as shown in equation (22), where Pij is the matrix of undetermined coefficients, t is the amount of time variation, j is the amount of change in the degree of freedom of the current joint of the robot, i is the order of the function, and n is the order of the function to be determined:(22)τi′t=Ψt=∑j=0nPij⋅tj.It can be seen that equation (22) is an implicit function of the kinetic model correction function. In order to obtain an explicit solution of the function, a mathematical model and an actual model are needed to define and solve the corresponding parameters.In order to find the difference between the solid model and the dynamic model of each joint, we subtract the response value of the dynamic model from the response value of the solid model. The resulting model difference is shown in Figure7, and the difference in the error of each joint portion is shown in Table 4.Figure 7
The actual difference curve of the solid model and mathematical model. Torque correction amount associated with timet of each joint.Table 4
Torque correction amount associated with timet of each joint.
Number
Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6
1
2386.993
6350.648
−6500.23
−285.029
243.4248
30.51686
2
963326.4
1605860
−1.2E + 07
−4.9E + 07
−6322412
3456341
3
1490851
1763153
−1.1E + 07
−4.7E + 07
−5900607
3269695
4
1604890
521430.1
−6484790
−4.5E + 07
−5056274
3164968
5
1553503
−1710918
684622.4
−3.9E + 07
−2993026
2884595
6
385449.2
−8416316
14053210
−9698177
2759736
1795244
7
−919109
−1.1E + 07
18790044
6699360
3397692
1746705
8
−1696889
−1.1E + 07
21712862
17946281
2836632
1935051
9
−1469893
−1E + 07
21058280
24486418
2035476
2076084
10
−143465
−8416636
15059756
29383435
1141529
2232086According to the data of Table4, it was found that the fitting result was better when j was equal to 5. Then, the undetermined coefficient matrix Pij under the task is expressed by equation (23). The matrix is 0 to 5 power-related from the bottom to top:(23)Pij=1.5E+06−7.28E+06−1.4E+07−3.7E+072.6E+062.2E+06−4.0E+063.5E+077.9E+072.0E+08−6.0E+06−1.4E+072.9E+06−4.9E+07−1.9E+08−4.1E+08−1.5E+073.3E+07−4.5E+059.8E+072.2E+074.5E+074.7E+07−3.6E+06−4.5E+069.8E+062.2E+084.5E+084.7E+07−3.6E+075.2E+066.1E+06−8.5E+07−2.0E+08−2.5E+071.5E+07.According to equations (22) and (23), the correction function of the dynamic model at each joint is shown in equation (24). It is shown in equation (25) that the total expressions of the correction function are from joint 1 to joint 6. Furthermore, the improved dynamic model equation is equation (26), thereby completing the improvement of the accuracy of the dynamic model:(24)τi′t=Ψt=∑j=0nPij⋅tj,(25)τ′t=τ1′tτ2′tτ3′tτ4′tτ5′tτ6′tT,(26)τ=τj+τ′t.According to the data in Table4 and Figures 5 and 6, the percentage improvement of the correction amount of such points compared with the original model equation is shown in Table 5, and the average increased percentage of joint model accuracy is shown in Table 6. The comprehensive improvement rate of the corrected model accuracy is 34.10%.Table 5
The increase percentage such points of joint model accuracy.
Number
Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6
Point 1 (%)
29.84
27.11
28.82
30.33
28.32
30.24
Point 2 (%)
33.62
33.82
30.83
34.19
34.31
34.82
Point 3 (%)
37.89
36.81
36.82
37.18
34.87
35.83
Point 4 (%)
35.61
37.84
37.91
35.42
36.77
36.91
Point 5 (%)
36.31
35.42
38.33
33.03
37.72
37.33
Point 6 (%)
33.23
30.03
35.44
32.17
36.33
34.01
Point 7 (%)
37.22
34.37
34.08
34.17
35.13
33.23
Point 8 (%)
34.32
32.33
33.13
34.33
32.15
33.68
Point 9 (%)
30.82
33.05
34.73
34.43
32.78
32.14Table 6
The average increased percentage of joint model accuracy.
Number
Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6
Percent (%)
34.32
33.42
34.45
33.92
34.26
34.24


## 7. Conclusion

This paper proposes an improved dynamic model for the extracorporeal ultrasonic lithotripsy medical cooperative robot, which can effectively improve the accuracy of dynamic models in task execution. Based on the Kane method, the dynamic model of the collaborative robot is established, and the solid model is built based on the collaborative robot. By comparing the difference between the dynamics model and the solid model, the improved dynamic model is established for the cooperative robot. After fitting, the correction function is found to be the best when the orderj of the undetermined coefficient is equal to five. Compared with the original dynamic model, the accuracy is improved by 34.1%, which provides a theoretical model for the use of the extracorporeal ultrasonic lithotripsy medical cooperative robot dynamic model.



---

*Source: 9752040-2020-04-10.xml*