Optimization of trajectory tracking control of 3-DOF translational robot use PSO method based on inverse dynamics control for surgery application

1. Introduction

The parallel mechanism robot has many good characteristics such as: high stiffness, high performance, high speed, and high accuracy. The Delta-type parallel mechanism robot, which was first proposed by Clavel are commonly used in food industrial, medical surgery and other high speed applications. Up to now, mechanical structure, kinematics, dynamics and intelligent control of Delta-type robot has been studied and developed worldwide. The kinematics of Delta robot was solved by applying vector loop equations and the dynamics equations were built by applying the Lagrangian equation of the first type [1]. Hamid D. Taghirad was discussed about kinematics, dynamics and control of parallel robot. Despite the fact that the controlling of parallel robot was mentioned, however, author have given some briefly block paragraph of controllers. These controller is applied to control planar manipulator and Stewart-Gough platform [2]. The most common control method for trajectory tracking on Delta robot is to use PID controller. To maximize appropriate workspace, Pradya Prempraneerach has researched and proposed the mechanical structure of Delta robot and using the traditional PID controller to tracking trajectory of the robot [3]. Sheng Jian et al. modeled the PMSM motor and used PID to control the motion of PMSM motor at three actuator joints of Delta robot [4], the gains of PID controller were set manual. In [5], L. Angel and J. Viola performed the trajectory tracking using two PID controller. Authors used Integer Order PID (IOPID) and Fractional Order PID (FOPID) base computed torque control technique and proposed an algorithm to find gains of the controller by using Matlab function. The fuzzy method is used by many authors in [6]-[9] to control the motion of Delta robot. Mohsen Asgari et al. performed dynamics modeling and trajectory tracking control using four methods as PD, PID, fuzzy PD, and fuzzy PID [6]. The comparison between four control method show errors of actuated joints using PID and fuzzy PID are the same (0.008rad) and less than results of PD and fuzzy PD. These errors increased in the presence of disturbance. Similarly, other researches [7]-[9] using fuzzy control show big amount of calculation and large error. Trajectory tracking of Delta robot were implemented by Neural Network based controller [10]-[13]. Similar to fuzzy method, these researches need a big amount of experimental data to train the controller. This defect lead to the increase of time to converge to the reference trajectory. It took more than 1s to reach the reference trajectory [10]. In [14]-[18] the authors applied the robust control method H∞ for motion control of Delta robot. These researches designed the H∞ robust controller and applied to the Delta robot to perform pick-and-place operation. The performance of proposed controller was compared to other controller as Computed Torque Control and PID. Simulation results show that the H∞ controller performed better than others. However, the steady state error of moving platform still exist and be unstable (maximum value more than 1mm). The Computed Torque Control technique is used in many researches [19, 20]. In this technique inverse dynamic is use to compensate nonlinear of the robot’s dynamic. This controller also called Inverse Dynamic Controller (IDC). Jianlong Hao et al. established the dynamic equation of Delta robot based on the principal of virtual work and proposed CTC to control trajectory tracking [19]. Extended CTC used extra data from additional sensors at passive joints of the Delta robot [20]. The performance of extended CTC was compared to the traditional CTC. In both researches above, the gains of controller have chosen manual. Luis Angel Castañeda et al. were designed an adaptive controller to solve the trajectory tracking problem of Delta robot with uncertain dynamical model [21]. They have simulated and compared the results with two other controller as PID and GPI. In [22], Joao Fabian presented the implementation of trajectory tracking of Delta robot using PD and LQR controller, where the PD controller parameters were obtained using pole placement. However, authors have not proposed any optimal trajectory tracking control method to manipulate the 3-DOF Translational Delta Robot type in the surrey application yet. In this research, authors focus on optimal mechanism and kinematics of 3-DOF Translational Delta Robot type to enlarge workspace by vary arm lengths. The trajectory tracking was simulated by using PID controller with various payload to demonstrate the pick-and-place operation. To optimize the trajectory of the 3-DOF translational robot, Particle Swarm Optimization method is chosen as a novel optimization trajectory tracking control for 3-DOF translational Delta robot type. PSO is a population-based optimization technique inspired by the motion of bird flocks and schooling fish. PSO shares many similarities with evolutionary computation techniques. The system is initialized with a population of random solutions, and the search for the optimal solution is performed by updating generations. In PSO, the potential solutions of particle swarm optimization for solving various kinds of optimization problems, called particles, move in the problem space by following the current optimum particles. PSO is computationally more efficient in terms of both speed and memory requirements.

This paper organizes like this: presents an optimal trajectory tracking control method for 3-DOF translational robot Delta type. The 3-DOF translational robot with three translational degrees of freedom has chosen. First of all, mechanism, kinematic and dynamic of robot are presented. Secondly, based on the dynamic of 3-DOF translational robot, an inverse dynamic controller (IDC) is designed to track a desired trajectory. To achieve the best performance of trajectory control, the Particle Swarm Optimization (PSO) is used to determine parameters of the controller. In this optimization method, Integral of the Square of the Error (ISE) play an important role as objective conditions. The simulation results are presented and compared with other control methods in the rest of this article.

2. Mathematic of kinematics and dynamics of 3-DOF translational robot

2.1. Kinematic of 3-DOF Translational robot

The 3-DOF translational robot has three arm connect the moving platform and the fixed base is shown in Fig. 1. Each arm have two parts, the upper arm is connected to the active motor using revolute joint. Each lower arm is made up of a four-bar parallelogram which connect with the moving platform and the upper arm using spherical joint. The moving platform can perform translational movement in three directions. In other words, the robot is called three translational degrees of freedom.

Fig. 13D drawing of 3-DOF translational robot

Fig. 2Vectors description of 3-DOF translational robot

Kinematics of 3-DOF translational robot has discussed by many authors with many methods, in this study authors chose the vector loop equation method to solve this problem. In [1], authors proposed the inverse kinematics of Delta parallel robot base on vectors loop equation. Vectors of 3-DOF translational robot are illustrated in Fig. 2. As shown on Fig. 2, the vectors loop equation of parallel robot is given as follow:

1

$l_{1i}{u}_{1i}+l_{2i}{u}_{2i}=p+b-a.$

Expressing Eq. (1) in the $\left(x_i,y_i,z_i\right)$ frame with $i=\mathrm{1,2},3$ yield:

2

$\left[\begin{array}{c}{l}_1\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{1i}+l_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_{1i}+{\theta }_{2i}\right)\\ l_2\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{3i}\\ l_1\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{1i}+l_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\mathrm{s}\mathrm{i}\mathrm{n}\left({\theta }_{1i}+{\theta }_{2i}\right)\end{array}\right]=\left[\begin{array}{l}{c}_{xi}\\ c_{yi}\\ c_{zi}\end{array}\right],$

where $l_1,l_2$ are length of lower arm and upper arm; ${\theta }_{1i},{\theta }_{2i},{\theta }_{3i}$ with $\left(i=\mathrm{1,2},3\right)$ are actuated joint angles and passive joint angles respectively, $c_{xi},c_{yi},c_{zi}$ are position of point $C_i$. The position of $C_i$ can be obtained as:

3

$\left[\begin{array}{l}{c}_{xi}\\ c_{yi}\\ c_{zi}\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}{\phi }_i& \mathrm{s}\mathrm{i}\mathrm{n}{\phi }_i& 0\\ -\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_i& \mathrm{c}\mathrm{o}\mathrm{s}{\phi }_i& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}{p}_x\\ p_y\\ p_z\end{array}\right]+\left[\begin{array}{c}a-b\\ 0\\ 0\end{array}\right],$

where $p_x,p_y,p_z$ are position of the center point of moving platform in the $\left(x,y,z\right)$coordinate system. The solutions of ${\theta }_{1i},{\theta }_{2i},{\theta }_{3i}$ are found by solving Eq. (2).

Solving Eq. (2) obtained:

4

${\theta }_{3i}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{c}\mathrm{o}\mathrm{s}\left(\frac{c_{yi}}{l_2}\right),$

5

${\theta }_{2i}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{c}\mathrm{o}\mathrm{s}\left(\kappa \right),$

where $\kappa =\left(c_{xi}^2+c_{yi}^2+c_{zi}^2-l_1^2-l_2^2\right)/\left(2l_1{l}_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\right)$.

Substituting Eq. (4) and Eq. (5) into Eq. (2):

6

$l_1\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{1i}+l_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\mathrm{s}\mathrm{i}\mathrm{n}\left({\theta }_{1i}+{\theta }_{2i}\right)=c_{zi},$

7

$l_1\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{1i}+l_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_{1i}+{\theta }_{2i}\right)=c_{xi}.$

Solving two equations above yield:

8

${\theta }_{1i}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\frac{-c_{xi}{g}_{2i}+c_{zi}{g}_{1i}}{c_{zi}{g}_{2i}+c_{xi}{g}_{1i}},$

where $g_{2i}=l_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{2i}$, $g_{1i}=\left(l_1+l_2\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{3i}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{2i}\right)$.

3. Optimization trajectory tracking controller of 3-DOF translational robot

3.1. Controller design

The dynamics Eq. (11) can be rewritten in matrices form as below:

13

$M\left(q\right)\ddot{q}+C\left(q,\dot{q}\right)\dot{q}+G\left(q\right)=\tau +{\tau }_d,$

where $M\left(q\right)$,$C\left(q,\dot{q}\right)$, $G\left(q\right)$ are mass matrix, Coriolis and Centrifugal matrix, gravity vector respectively, ${\tau }_d$ is disturbance.

The Inverse Dynamics control law of 3-DOF transnational robot is written as [19]:

14

$\tau =\widehat{M}\left(q\right)\left({\ddot{q}}_d+K_{p}e+K_d\dot{e}\right)+\widehat{C}\left(q,\dot{q}\right)\dot{q}+\widehat{G}\left(q\right),$

where $\widehat{M}\left(q\right),\widehat{C}(q,\dot{q}),\widehat{G}\left(q\right)$ denote the approximate estimate of the manipulator mass matrix, Coriolis and Centrifugal matrix and gravity vector in the joint space respectively; $q_d$ and ${\ddot{q}}_d$ are joint angle and joint acceleration desired; $K_d$, $K_p$ are positive diagonal matrices.

Substituting Eq. (9) into Eq. (8) the dynamics equation of 3-DOF translational robot can be expressed as follow:

15

$M\left(q\right)\ddot{q}+C\left(q,\dot{q}\right)\dot{q}+G\left(q\right)=\tau +{\tau }_d$
$=\widehat{M}\left(q\right)\left({\ddot{q}}_d+K_{p}e+K_d\dot{e}\right)+\widehat{C}\left(q,\dot{q}\right)\dot{q}+\widehat{G}\left(q\right)+{\tau }_d.$

In the dynamics equation of 3-DOF translational robot $C(q,\dot{q})=0$ thus $\widehat{C}(q,\dot{q})=0$. In real model, the $M\left(q\right)$ and $G\left(q\right)$ can calculated, assume that $\widehat{M}\left(q\right)=M\left(q\right)$, $\widehat{G}\left(q\right)=G\left(q\right)$ and there is no disturbance wrench is applied to the manipulator, the error dynamic is simplified to:

16

${\ddot{q}}_d+K_d\dot{e}+K_{p}e=0.$

With error dynamic equation Eq. (16) above, the tracking error can be designed to convergence toward zero by choosing $K_d$, $K_p$ appropriate. The Inverse Dynamics controller model in joint space is depicted in Fig. 3.

This section may be divided by subheadings. It should provide a concise and precise description of the experimental results, their interpretation, as well as the experimental conclusions that can be drawn.

Fig. 3Trajectory tracking inverse dynamics controller

4. Simulation results and discussion

The simulation of kinematic, dynamic and control of the 3-DOF translational robot was implemented in Matlab/Simulink. Dynamic parameters of the robot are described in Table 1.

As mention above, corresponding to dynamic parameters of the robot, the $K_d$, $K_p$of the controller was found by applying the PSO method, we have:

The trajectory form was designed by using the third odder polynomial as below [23]:

The coefficients of Eq. (14) are obtained by solving set of equations Eq. (15):

where $q\left(t_0\right)=q_0$, $\dot{q}\left(t_0\right)=v_0$, $q\left(t_f\right)=q_f$, $\dot{q}\left(t_f\right)=v_f$ and $t_0$, $q_0$,$v_0$ are initial time, position and velocity respectively, $t_f$, $q_f$,$v_f$ are final time, position and velocity respectively.

In this research, the trajectory of the moving platform is implemented in $OX$ direction which relative to the $\left(x,y,z\right)$ coordinate system, where $t_0=0$, $t_f=$ 10 (sec), $q_0=0$, $q_f=$250 (mm) and $v_0=0$, $v_f=0$. The trajectory, velocity and acceleration are showed in Fig. 4.

Fig. 4Trajectory planning

a)

b)

c)

The numerical simulation results of the joint angles of three arm are shown in Figs. 5(a-c). The torque of three actuators joint are presented in Fig. (d).

Fig. 5Torque of three actuator joint

a)

b)

c)

d)

As mention at the first part, the objective condition of optimization method is integral of the square of the error $J_{ISE}$. The PSO will apply to find $K_d$, $K_p$which corresponds to minimum of $J_{ISE}$ [24]:

where $e_{ss}$ is steady state error of actuator joint of the robot. According to parameters and the trajectory planning of the Delta robot, the steady state error of three actuators joints are showed in Fig. 6. Where $J_{ISE1}=$ 0.2555, $J_{ISE3}=$ 0.2557 respectively.

Fig. 6Steady state error of three actuator joint

The performance of proposed control method could be evaluated by compare with other methods in [21] as shown below in Fig. 7.

Fig. 7Comparing angle error with other methods

a) Error produced by IDC method optimize by PSO

b) Red line is error produced by PID controller, blue line is error produced by GPI controller (base on extended state observer), black line is error produced by adaptive controller proposed in [21]

Fig. 7 shows the error of actuator joint 1 of the proposed controller and other controllers implemented in [21]. Fig. 7(b) shows that all results still oscillate around zero points with 0.005 rad deviation. The result of adaptive controller is the best of three methods but still restricted to a vicinity of the origin. Fig. 7(a) shows the error produced by optimization IDC is better than other methods. The graphs show that the error converges to zero after 0.2 s and the steady-state error is maintained no more than 5.10-5 rad. Another comparison can be performed between the proposed controller and the controller in [19], the graph of errors as showed in Fig. 8.

Fig. 8Comparing Errors of actuator joints of optimization a) IDC and b) PD-CTC [19]

a)

b) The maximum error of joints at 1 degree (0.017 rad) while in a) as mention above, the error is 5.10-5 rad

Fig. 8 show the error of three actuators joint angle of the 3-DOF translational robot. In Fig. 8(a), the error curve converges to zero after 0.1sec, and the steady state error is very small, while in Fig. 8(b), this value cannot converge to zero, even it oscillates around –1 and 1 degree. The comparisons above demonstrates the better performance of this controller in accuracy and stability.

5. Conclusions

In this paper, the trajectory tracking of 3-DOF translational robot problems was implemented and optimized by used the Inverse Dynamics Control (IDC) method and Particle Swarm Optimization (PSO) algorithm. The IDC is used to track the desired trajectory in joint space. The IDC is a novel and effective method for 3-DOF translational robot control systems in the surgery field. PSO algorithm to find out the gains of controller more quick and simple than other methods. The comparison of simulation results showed the steady-state error of the proposed method is smaller and converge to zero faster than other methods. In the future, this control method will be used to combine with other force control to improve the control of the 3-DOF translational robot in the real model.