Trajectory Tracking Control of UR5 Robot Manipulator Using Fuzzy Gain Scheduling Terminal Sliding Mode Controller

: In dealing with the trajectory tracking control of robotic manipulator many scholars have had to work in a sliding mode controller. However, SMC has two major limitations one is chattering the other is asymptotical convergence. The chattering problem occurs in traditional SMC due to the switching function of the discontinuous controller and the constant gain parameter of K. Though in this study a Fuzzy Gain Scheduling Terminal Sliding Mode (FGSTSM) controller with a hyperbolic tangent function instead of signum function, is proposed and applied for tracking control of the UR5 robot manipulator. Moreover, the mathematical model of the UR5 robot manipulator using the Newton-Euler algorithm was developed on Maple software of version Maple 18.2. Hence the trajectory tracking control of the UR5 robot manipulator simulation was conducted on MATLAB/SIMULINK of version Matlab R2016a. Then for simulation purposes, a curve (arc) was taken as the desired trajectory for the robot manipulator to track and finally the result is compared with the conventional SMC controller.


Introduction
Robots have been traditionally employed in closed environments separate from human beings because they have been heavy, bulky and dangerous. In recent years, a lot of development has been made to robots that are safe enough to interact with human beings. In the future, one might envision service robots co-existing with humans. Such robots are known as Collaborative Robots (COBOTS). Universal Robot is the most prominent type of manipulator manufacturer in the collaborative robot world. A series of robotic manipulators including UR3, UR5 and UR10 were developed by the universal robot and now they are widely used in different applications. For instance, this robot is widely used in industry, in academic and now researchers tried to implement it in hospitals for COBOT surgery.
Trajectory tracking control of robot manipulators plays an important role in the control theory due to the wide development of engineering practice. The trajectory tracking control or the motion control of the robotic manipulators have a very significant influence on the overall manipulator performance. Robotic manipulators are always exposed to unmodeled dynamics including unknown gravitational and friction torque, disturbances and uncertain inertia. All these un-modeled robotic manipulator parametric uncertainties are addressed by the kinematic and dynamic modeling of the robotic system.

Literature Review
Though to control such a highly disturbed, MIMO, coupled and non-linear dynamic system demand a controller which is robust against uncertainties, fast response and simple for design. Liu and Zhang (2012) have introduced a fuzzy sliding mode controller first for two Degrees Of Freedom (2 DOF) robot arm and then tested on 4 DOF SCARA. This controller first developed for under actuated SCARA robot. Though this article, used a sliding mode controller to improve the control accuracy and the robustness of the robotic manipulator and the fuzzy logic control was used to approximate various uncertainties and to alleviate the chattering effect of the SMC.
A simple fuzzy sliding mode control to achieve the best trajectory tracking for the robot manipulator was reported by (Soltanpour et al., 2014). The principal of the proposed method by the author, applied the feedback linearization technique, as a result, the known dynamics of the robot's manipulator is removed; and then, to overcome the remaining uncertainties, a classic sliding mode control was used.
3-DOF spatial manipulator trajectory tracking control was reported by (Wan et al., 2016). To realize a high accuracy trajectory tracking a novel sliding mode controller with PID was proposed in this article. This paper significantly attunes the chattering effect of the Sliding mode controller by introducing the switching function, however, the system stability in both globally and asymptotically analyzed theoretically. PSO based neurofuzzy sliding mode control strategy for two Degrees Of Freedom (2DOF) rigid robot manipulator was developed by (Vijay and Debashisha Jena, 2017). The formulation of a coupling Artificial Neuro-Fuzzy Inference System (ANFIS) with Sliding Mode Control (SMC) approach was the main concern of the paper. In the beginning, SMC with Proportional Integral Derivative (PID) sliding surface is adapted to control the robot manipulator, but the sliding surface parameters are obtained by minimizing quadratic performance indices using Particle Swarm Optimization (PSO).
Moreover, a Fractional-Order Nonsingular Terminal Sliding Mode (CFONTSM) based on Time Delay Estimation (TDE) approach was presented by (Liu and Wang, 2012) for practical tracking control robot manipulator. This control scheme doesn't require detailed information about the robot dynamics, leading to a better model-free nature of TDE, however, this paper considers only lumped uncertainties, under this condition FONTSM surface and fast-TSM-type reaching law ensure fast convergence and high tracking precision. Simulation and experiment was carried out using two degrees of freedom robot arm for the purpose of testing and validating the proposed controller. An adaptive robust sliding mode tracking controller for a 6 degree-of-freedom industrial assembly robot with parametric uncertainties and external disturbances was developed by (Pan et al., 2018). Then the proposed dynamic control algorithms have been designed and simulated in MATLAB toolboxes. However, the proposed control of this paper was compared with a commonly used Proportional Derivative (PD) controller with fixed control parameters and a conventional sliding mode controller without adaptive and robust terms.
Task space absolutely continuous Terminal Sliding Mode (TSM) controllers with the finite time convergence property of the trajectory tracking by n-DOF rigid robotic manipulator were proposed by (Galicki, 2016). The terminal sliding mode controller on this research, makes it possible to simultaneously apply both the first and second-order sliding mode controller with their advantages. This work addresses the problem of the accurate task space control subject to finite-time convergence and dynamic equations uncertainty of a rigid robotic manipulator was also considered.
Having the good characteristics of the sliding mode, researchers have focused on finding a new continuous SMC controller that can be converged in finite time globally. The discontinuous terminal sliding mode controller has been developed by (Yu and Zhihong, 1998;. However, this discontinuous TSM controller has been widely used for robotic manipulator applications. Nevertheless, this system is highly singular due to the fractional power in a negative error signal of the sliding variable of the TSM controller. To overcome the singularity problem related to the sliding function of the TSM controller, Nonsingular TSMC has developed by (Feng et al., 2002). Nonetheless in this study to reduce the chattering of the discontinuous control a boundary layer approach was applied, however, the finitetime stability property was lost. Latter on a continuous terminal sliding mode controller was developed by (Yu et al., 2005). This approach has tried to address global finite-time stability adequately. However in this study, a constant tuning switching gain has existed, this is another factor that contributes to the formulations of the chattering phenomenon.
Latter on a continuous terminal sliding mode controller was developed by (Yu et al., 2005). This approach has tried to address global finite-time stability adequately. However in this study, a constant tuning switching gain has existed, this is another factor that contributes to the formulations of the chattering phenomenon. Regardless of VSS in the above reviewed articles most controllers have some major problems have been considered for this study, among those the chattering phenomenon, asymptotic convergence and a constant or a fixed tunable reaching control gain (K) are mentioned. So to meet the demand, many scholars have been developed different types of controller for robotic manipulator. Though to overcome the above main drawbacks of a continuous finite time terminal sliding mode controller, in this study FGSTSM controller is developed for trajectory tracking control of UR5.
Design, modeling and control of lightweight and flexible robotic manipulators recently become more attractive among the robotics community because lightweight and flexible robotic manipulators have a great potential in the design of high performance industrial robotic manipulators since they allow highspeed operation and low energy consumption. Thus, in this research, the kinematic and dynamic model analysis of the Universal Robot 5 (UR5) robotic manipulator is developed by using MATLAB and MAPLE software. Henceforth a fuzzy gain scheduling TSM controller is adopted for trajectory tracking problems. Finally, the trajectory tracking control of the UR5 robotic manipulator will be tested by simulating the robotic system using the FGSTSM controller and the result will be verified by comparing the result with other conventional SMC controllers in different scenarios.

115
This article is organized as follows: Section 2 discussed the development of the kinematic and dynamic model of the UR5 robot. Section 3 formulates the conventional and the proposed fuzzy gain scheduling terminal sliding mode controller. Section 4 in this section simulation results will be discussed. Section 5 will conclude the paperwork.

Mathematical Model of 6-DoF UR5
The science that deals with motion without considering the force causes the motion is known as kinematics. Hence, the study of the kinematics of robotic manipulators refers to all the geometrical and time-based properties of the motion. Therefore, a kinematic model of the 6-DOF UR5 robotic man manipulator firstly developed.

Kinematic Modeling of UR5
From the mechanical structure point of view, the UR5 robot manipulator is an open kinematic chain as shown in Fig. 1, which connected by 6 revolute joints.
Robot arm always expressed kinematically by giving the values of four quantities for each link. The first two quantities describe the link itself and the other two represent the link's connection to a neighboring link. In the UR5 robot manipulator (i.e., all joints are revolute) the joint variable is θ, the other quantities are constant and called link parameters. Though, joint variables and the link parameters of the UR5 robot arm from Fig. 2 as presented by (Kebria et al., 2016 (2) The first 33 matrix represents orientation or the robotic manipulator, whereas the last column represents the position of the end-effector.

Dynamic Modeling of UR5 Robotic Manipulator
Modeling the dynamics of the robot manipulator means deriving a comprehensive relation between force and motion. The robot dynamic behavior can be expressed in terms of robot configuration and its derivatives in relation to the joint torques. This relation can be described by a set of differential equations, known as the equation of motion (dynamic model) which governs the dynamic response of the whole robotic system. There are two different algorithms, to develop the dynamic model of the robotic manipulator: The Newton-Euler and the Lagrange-Euler algorithm. This paper will drive the dynamic model of UR5 robot manipulator based on the Newton-Euler formulation.
Hence, the dynamic equation (equation of motion) develop the relationship between the joint torques , joints positions q, joint velocities q̇ and joint accelerations q̈ of the robot manipulator. Therefore the general equation of motion expressed in the form: where, the matrix M(q) is called the joint space inertial (mass) matrix, which gives the relationship between the joint force/torque and the resulting joints accelerations. The vector C(q̇, q) is the centrifugal and Coriolis vectors of torques and the vector g(q) is the vector of gravitational joint torques.
By rearranging the above equation of motion it is possible to find, the forward dynamics equation and expressed as: Numerous literature on analytical mechanics have provides several approaches to develop the dynamic equation for a robot manipulator. These approaches include recursive Newton-Euler (Luh et al., 1980), Euler-Lagrange Paul (1981), Kane's equation (Kane and Levinson, 1983) and so on. Thus, in this paper the dynamic model of the manipulator formulated based on the recursive Newton-Euler method is established in (Sciavicco and Siciliano, 2012) and (Luh et al., 1980) for the manipulators having revolute joints. Moreover, the dynamic formulation (the forward and backward recursion) which govern the robotic system computed by an automated software called Maple symbolically.

A Continuous Terminal Sliding Mode Controller Analysis
The non-linear controller provides an acceptable control performance with stability and robustness for a non-linear systems. Terminal Sliding mode control has the same robust properties like conventional sliding mode controller and which is one of a nonlinear type controller and is generally a non-linear control scheme and recognized as the most powerful, robust and simple controller.

System Description and Problem Formulation
Therefore, to recall the dynamic model of a robotic manipulator (i.e., six degrees of freedom) is obtained in the previous section and can be expressed as: In Equation (3) (the manipulator dynamics) represents the dynamic behavior of the robotic manipulator, but the expression is highly non-linear, coupled and complex second-order system of the form: where, = 1,2,3 …n. Let's consider as a state variables by assigning: ; , 1,2,..., Then, the state variables (state vectors) of the robotic manipulators can be defined in this form: where, t  R is the time, x(t)  R 2n is a state variables, u(t)  R n is the control input of the system and they are defined as: Therefore the dynamic model in Equation (3) of the robot manipulator can be expressed as:

Sliding Mode Controller Revision
Sliding mode controller derivation has two phases:  Phase 1 Sliding surface design  Phase 2 Control input design Therefore to discuss the first phase, first, let's define the sliding surface: A classic form for the sliding surface is the following, which depends on just a single scalar parameter : where, e and ė are position and velocity error respectively and defined as: Thus, the siding surface equation is defined as: where,  is a positive diagonal matrix. The next phase, (phase 2), compute the control action that drives the system trajectories on to the sliding manifold. In another way, the control is capable of drives the variable to zero in finite time.
Therefore in case of sliding mode control, the controller fulfills the following conditions:

118
In Equation (14) the sliding surface equation, the sliding surface can be accomplished when s(t) gets zero (Slotine and Li, 1991). The reaching condition expressed by the Lyapunov function candidate makes the state vectors approach the defined sliding surface. The Lyapunov function, in Sliding mode expressed as: Form the above Lyapunov function, the reaching condition is taken when: where, s(x, t) is switching function and ṡ (x, t) is a reaching law. A constant rate, exponential and a power rate reaching law is introduced by (Liu and Wang, 2012) and among those exponential reaching law is taken in these studies and which is expressed as: Though in this study an exponential reaching control law is selected for both conventional sliding mode controller and terminal sliding mode controller.
Then the total torque input u becomes as: Substitute the derived manipulator dynamics into the general second-order non-linear equation in Equation (13) and expressed in the form: The control input torque (t) expressed in the form: In the sliding surface or on the sliding plane where S(t) = 0 and Ṡ(t) = 0, the equivalent control torque o(t) is modeled to keep the states on the sliding phase and move along with it. In the reaching phase where S(t)  0 the discontinuous control torque C(t) is designed to satisfy the reaching phase condition.
The Continuous terminal sliding mode block diagram as shown in Fig. 3, the total control input torque (tot) to the manipulator is the sum of equivalent control torque and sliding mode control: Since, o(t) is an equivalent control torque and C(t) is the sliding mode control torque or the discontinuous part: Therefore, Equation (25) is consist of both the equivalent control and the sliding mode control torque and expressed as: This implies: and, the discontinuous control torque become as:

Terminal Sliding Mode Controller Design
The robotics activities in industries nowadays become more complicated and advanced, though to meet these requirements a new continuous terminal sliding mode is proposed in this paper with fuzzy gain scheduling controller. Therefore, a new continuous terminal sliding mode is developed in this paper to control the trajectory tracking of UR5 robotic manipulator with global finite-time stability is used. For the above second-order nonlinear system in Equation (22) 119 of the terminal sliding mode controller is taken as developed by (Yu et al., 2005): where, 1 <  < 2 and  > 0, Intended for simplicity some notations are introduced for the analysis of TSMC by (Haimo, 1986). Similarly as a sliding mode controller the control input or a control torque for terminal sliding mode controller is the same as given in Equation (25). Then the equivalent and the discontinuous controller is obtained as follow: The simplified form of terminal sliding function is expressed as: Then the next step is to solve the equivalent control torque on the sliding phase (Ṡ = 0). Then differentiating the terminal sliding variable will become as: Therefore an equivalent control torque can be expressed as: Moreover, for the reaching phase, an exponential reaching law is taken and exponential reaching control law is described as follow: Therefore, the discontinuous (the switching) control torque is obtained as: However, the main source of chattering is sign function in the discontinuous controller, though to come chattering, a hyperbolic tangent function (tanh) is adopted in the switching control instead of signum function and the equation rewritten as follow: Then finally the total control torque input can be expressed as: The Lyapunov stability and a finite-time convergence were also computed according to the Lyapunov stability function by (Yu et al., 2005). The controller scheme for the terminal sliding mode controller is depicted below in Fig. 3.

Fuzzy Inference System Design for Gain Scheduling of TSM Controller
If a nonlinear dynamics system plants are changes its operating point due to unknown uncertainties and disturbance, it may be conceivable to alter the parameters of the controller according to each operating point. This is known as gain scheduling. As it has been proven by many scholars that the fuzzy logic controller is appropriate for gain scheduling. The fuzzy logic controller consists of four main general steps of (i.e., fuzzification, inference rule and defuzzification).
The inputs for the fuzzy inference system are selected to be the terminal sliding variable S(t) and change in the terminal sliding variable Ṡ(t) and the output would become the switching gain K(t) as depicted in Fig. 4. The universal discourse for the inputs is taken from the results of the simulation response, whereas the output variable is taken by trial and error for the optimum position error and torque response. Form the input universal discourse of S(t) and Ṡ(t) the membership functions are designed for both inputs parameters as Negative Big (NB), Negative (N), Zero (ZO), Positive (P) and Positive Big (PB).
The fuzzy controller K(t) output membership function also classified as Very Small (VS), Small (S), Medium (ME), Large (L) and Very Large (VL). All input membership functions are taken as triangular and uniformly distributed shape. Thus in this paper, a Prod-Sum and a constant Takagi Sugeno Fuzzy inference system are used to design the rule base.
The linguistic controller contains rules in the if-then format is designed on MATLAB/Simulink according to the existence of the sliding mode (S(t) Ṡ(t) < 0). Though as long as the state trajectories are on the sliding mode the switching gain should be small, whereas when the states are far from the sliding mode due to unknown model uncertainties and disturbance the switching gain should be increased.
The surface of the rule base for the six sliding variable and its derivatives are depicted as follows.

Simulation Result and Discussion
The level-2 MATLAB S-function API allows the any user to create custom block by using MATLAB R2016a language with multiple inputs and multiple outputs. Thus, the dynamics of the UR5 robot manipulator is implemented in SIMULINK using level-2 MATLAB S-Function.
For solving the differential equations commonly ode 45 is used, but sometimes automatically it switched to ode 15 solver. At each step time of the simulation, the state vector is sent from SIMULINK to the MATLAB interface via to workspace block. Finally, the simulation result is plotted graphically, by Scope block or 3D Graph.

Open-Loop Simulation
In most cases of the open-loop system as demonstrated in Fig. 5, the output response has no effect on the input to the control process. Therefore, the velocity and position outputs of the UR5 robot manipulator with zero torque input is studied. Thus, simulation was initially done under a 121 zero torque input situation. As a result unlike the other joints, joint 5 and 6 shows highly disturbed and chaotic properties as shown in the Fig. 6. Zero torque input is taken as: The result of the instability behavior of the two joints is caused by gravitational force. Therefore, applying gravity compensating torque for the manipulator will result in a stable or undisturbed system response as shown in the Fig. 7.
The gravity compensating torque is found as:

A Sliding Mode Controller for Tracking Control of UR5
The Sliding mode controller internal structure is also depicted in Fig. 8 and under this block basically, there is two main control gains which is the sliding surface gain and the reaching control gain and the saturation function block.
Moreover, the same joint trajectory path is used for both (i.e., sliding mode controller and for fuzzy gain scheduling terminal sliding mode controller) and the initial and final joint positions are expressed as follows: The sliding Surface gain and the reaching control gain is taken as following for trajectory tracking control of the UR5 robot manipulator: Finally, after the simulation is done, the 3D graph plot the actual and desired trajectory path of the robot manipulator, here in the Fig. 9 it seems both having the same color, however, the graph is consists of the blue line and red line, where the blue one is the reference input trajectory while the red line represents the actual path of the robot manipulator.
Then the results of the position error, velocity error and the total control input torque in nm to the system are depicted as follows.
As can be seen in the above-depicted diagrams, on the position and velocity error response of the UR5 robot manipulator in Fig. 10 to 12 resulted in an unsatisfactory error convergence in finite time, however in the 3D graph of the actual and the desired trajectory path looks accurately matched. Nevertheless, the system error exhibited large error relative to the terminal sliding mode controller. The sliding function of the traditional sliding mode control also becomes chatter due to a high frequency switching action of the discontinuous controller.
The control torque input in Fig. 13 also has a chattering phenomenon, which is an undesired and unwanted condition on the traditional sliding mode controller due to a high frequency action of the discontinuous controller, the chattering phenomenon is most difficult to implement practically on the actuator. This problem is caused by two main reasons one is tuning the switching controller constantly at sliding mode and at reaching condition when the system is at sliding mode the switching control gain should be small or less, whereas when the system at reaching condition the switching gain should become large to overcome disturbance.
The second case that produces chattering problem is taking the linear sliding function. Though to alleviate the above major problems, next a fuzzy gain scheduling terminal sliding mode controller is used and simulated the result in the next coming chapter.
Note that in Fig. 14 qa stands for actual angular positions and qd represents the desired angular position of the applied to the joints in rad.) Such that, in Fig. 15, Xa, Ya, Za stands for actual angular positions and Xd, Yd, Zd represents the desired angular position of the applied to the joints in meter.

Fuzzy Gain Scheduling Terminal Sliding Mode Controller for Tracking Control of UR5
In this section, a fuzzy gain scheduling controller is tested to control the trajectory of UR5 robot manipulator. The basic control approach of FGSTSM controller is reviewed in the previous chapter. As per the mathematical formulation, the FGSTSM controller was implemented in Simulink. The performance of the controller also measured as the position and velocity error and the control input torque. As per the mathematical formulation, the FGSTSM controller was implemented in Simulink. The performance of the controller also measured as the position and velocity error and the control input torque.
The sliding Surface gains of a terminal sliding mode controller is taken as follow: 132 However, switching control gain K1 is obtained and tuned by fuzzy inference system adaptively and the switching control gain K2 are tuned manually and the tuning parameters K2 are used as the given below: In Fig. 22 the broken lines represent the reference x, y, z position, whereas the solid lines of the graph indicate the tracked linear position of the UR5 manipulator.
In Fig. 23 all the broken lines represent the actual tracked joint position, whereas the solid line represents the reference joint position of the UR5 robot manipulator. Fig. 17 showed the results of the actual and the reference trajectory path that the manipulator has moved on under the FGSTSM controller. Hence the red colored line represents the actual path that the robot moved whereas the blue one is the reference trajectory path given to the plant.
In this study the conventional SMC controller and the developed Fuzzy gain Scheduling Terminal sliding mode controller have compared, accordingly the FGSTSM controller exhibits a fast tracking under equal simulation time, a better tracking accuracy and alleviate the chattering problem. Various responses of the proposed fuzzy gain scheduling terminal sliding mode controller are presented above. Firstly as it is shown in the position error graph in Fig. 18 of the FGSTSM controller the position error converges to the equilibrium in finite time.
Moreover, the other two basic responses from the trajectory tracking control of UR5 using FGSTSM controller are the sliding function ( ) and its derivatives () of TSMC. The response of the sliding function and its derivatives of TSM controller are demonstrated in Fig. 19 and Fig. 20 respectively. As it can be seen clearly both exhibits a small amounts of steady state error as well as fast finite time convergence.
The position error also highly minimized when it is compared to the sliding mode controller. The other prominent contribution of the proposed FGSTSM controller is alleviating the chattering problem in the system. The chattering phenomenon is eliminated by tuning the gain adaptively using the fuzzy inference system and using the nonlinear sliding variable (i.e., terminal sliding mode controller) as shown in the Fig.  21. The other main advantage of tuning the switching control gain are used to handle system uncertainties by increasing the switching gain automatically. However, as it is reviewed in the previous chapter the implementation of the FGSTSM to the tracking control of the UR5 robot manipulator is relatively complicated.
In this study the conventional SMC controller and the developed Fuzzy gain Scheduling Terminal sliding mode controller have compared, accordingly the FGSTSM controller exhibits a fast tracking under equal simulation time, a better tracking accuracy and alleviate the chattering problem. He et al. (2016) developed AFSM controller with nonlinear observer for redundant manipulator. The developed control approach have resulted a superior output in tracking performance and alleviating the chattering phenomenon. However, the main shortcoming of this scholars is related to the tuning parameters of the controller to attune uncertainties, hence in order to get a better controller performance they have increased the sliding mode and the reaching mode controller gain to the highest tuning parameter, sometimes this scenario might be impractical. Neila and Tarak (2011) an adaptive terminal sliding mode controller was developed for robotic manipulator, adaptive control algorithm was utilized to estimate bounds of uncertainties, this leads the controller approaches to be complex. Though the tracking error and the chattering problem bring about satisfactory for 2 DOF robot arm.  Mustafa et al. (2017) unlikely this paper was tried to remove the effect of lumped uncertainties and disturbance for trajectory tracking of robotic manipulator. Under this work the control torque input wasn't considered.

Conclusion
In this study, a new fuzzy gain scheduling terminal sliding mode controller was developed for the tracking control UR5 robotic manipulator. After validating the open loop system stability, trajectory tracking control by the conventional sliding mode controller and FGSTSM controller was developed and simulated. After validating the open-loop system stability, trajectory tracking control by the conventional sliding mode controller and FGSTSM controller was developed and simulated. Simulation results demonstrate the proposed FGSTSM controller is effectively alleviated the undesired chattering in the torque input to the manipulator ensuring a better tracking performance. On the other hand, the proposed controller successfully tune the switching gain K adaptively as demonstrated in the Fig. 16, this definitely will help the controller to handle undesired uncertainties as well as helpful for alleviating chattering.
In this study, the hyperbolic tangent function was used in the discontinuous (switching) controller for FGSTSM control instead of sign function, which also has contributed a lot in smoothing the control signal. Hence, in general, the conventional SMC has resulted in chattered control input due to a constant switching gain and the sign function of the discontinuous controller in moderate tracking error. Moreover, SMC converges to the equilibrium point asymptotically. Whereas in the proposed controller the chattering problem in the control input is completely alleviated and the state trajectories converge to the equilibrium points with global and finite-time stability.