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

This research presents an optimal trajectory tracking control method for improving the accuracy of 3-DOF translational parallel robots in the surgery field based on the Particle Swarm Optimization (PSO) method. The 3-DOF translational robot has three translational degrees of freedom, which consists of three arms with three revolute joints and twelve spherical joints. Firstly, the kinematics model is established; and the dynamics equation of the Robot is built by applying the Lagrange equation of the first type, and then the dynamics controller of 3-DOF translational robot is designed base on the dynamics equations. Secondly, a trajectory tracking controller model using the Particle Swarm Optimization based on inverse dynamics controlled method for 3-DOF translational Robot is designed. The control performance results of the proposed controller is evaluated by simulation and compared with the other published research results. Finally, the proposed controller can achieve better tracking performance in comparison with other controllers as Proportional Integral Derivative (PID) controller, GPI controller, and adaptive controller.


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.

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.

Dynamics equation of 3-DOF translational robot
In this research author using the Lagrangian formulation of the first type to solve the dynamics problems of 3-DOF translational robot. The Lagrangian function of the first type is written as [1]: where Γ denotes the th constraint function, is the number of constraint functions, is the Lagrangian multiplier. is the Lagrangian function. Constraint equations Γ with = 1,2,3 are obtained from the fact that the distance between joints B and C is always equal to the length of the connecting rod of the upper arm: Taking the derivatives of contraint functions and Lagrangian function, Lagrangian multipliers, the dynamics of 3-DOF translational robot can be written as: = + + + + cos −2 cos + sin + − sin − cos , where is axial moment of inertia of the rotor mounted on the th limb, is moment of lower arms, , , are mass of moving platform, lower arm and upper arm respectively Eq. (11) can be rewritten in matrix form as following: where: is the manipulator inertia matrix, ( ) is the gravitational vector, is the vector of generalized forces, is vector of generalized Lagrange coordinates:

Controller design
The dynamics Eq. (11) can be rewritten in matrices form as below: where ( ), ( , ), ( ) are mass matrix, Coriolis and Centrifugal matrix, gravity vector respectively, is disturbance. The Inverse Dynamics control law of 3-DOF transnational robot is written as [19]: where ( ), ( , ), ( ) denote the approximate estimate of the manipulator mass matrix, Coriolis and Centrifugal matrix and gravity vector in the joint space respectively; and are joint angle and joint acceleration desired; , are positive diagonal matrices. Substituting Eq. (9) into Eq. (8) In the dynamics equation of 3-DOF translational robot ( , ) = 0 thus ( , ) = 0. In real model, the ( ) and ( ) can calculated, assume that ( ) = ( ), ( ) = ( ) and there is no disturbance wrench is applied to the manipulator, the error dynamic is simplified to: With error dynamic equation Eq. (16) above, the tracking error can be designed to convergence toward zero by choosing , 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.

Particle Swarm Optimization (PSO) algorithm
This paper is presented a method of choosing the parameters of the Inverse Dynamics Controller. The dynamics error equation reach to zero by choosing , gains. In this research, the authors are using the Particle Swarm Optimization method to find the optimization gains. The advantages of PSO are: simplicity and efficiency, proven in many other parameters training problems. The PSO method was developed by simulation of simplified social model, where each population is called a swarm. Each candidate, called a particle, flies through problem space to look for the optimal position, similar to food searching of bird swarm. Assume that, the search space is n-dimension, each particle corresponding to an -dimension position vector where is inertia weight; , are random variables in the range of 0 1 ; , are positive constant parameters called acceleration coefficients. In this paper, the PSO was combined with the kinematic and dynamic model of 3-DOF translational robot to find out the gains of controller. The parameters was chosen as = 0.9, = 0.12, = 1.2, = 50, = 50. The fitness function is the measurement of the trajectory's quality. The objective functions applied for this particular problem is the Integral Square Error (ISE). In this case, the best position is determined by the minimum of Integral of the Square of the Error value by using Matlab toolbox. In each iteration, the gains are updated sequentially in individual best position set and global best position set corresponding to ISE value. The gains are taken after 50 iterations show the errors are acceptable as mention in simulation part below.

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 , 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  torque of three actuators joint are presented in Fig. (d). Fig. 5. Torque of three actuator joint As mention at the first part, the objective condition of optimization method is integral of the square of the error . The PSO will apply to find , which corresponds to minimum of [24]: where 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 = 0.2555, = 0.2557 respectively.   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.   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. Comparing Errors of actuator joints of optimization a) IDC and b) PD-CTC [19] 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.

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.