Dynamic Modeling and Torque Feedforward based Optimal Fuzzy PD control of a High-Speed Parallel Manipulator

— Dynamic modeling and control of high-speed parallel manipulators are of importance due to their industrial applications deployed in production lines. However, there are still a number of open problems, such as the development of a precise dynamic model to be used in the model-based control design. This paper presents a four-limb parallel manipulator with Schönflies motion and its simplified dynamic modeling process. Then, in order to fix the issue that computed torque method control (CTC) will spend a lot of time to calculate dynamic parameters in real-time, offline torque feedforward-based PD (TFPD) control law is adopted in the control system. At the same time, fuzzy logic is also used to tune the gains of PD controller to adapt to the variation of external disturbance and compensate the un-modeled uncertainty. Additionally, bottom widths of membership functions of fuzzy controller are optimized by bat algorithm. Finally, three controllers of CTC, TFPD and bat algorithm-based torque feedforwad fuzzy PD controller (BA-TFFPD) are compared in trajectory tracking simulation. Fro the result, compared with TFPD and CTC, BA-TFFPD can lead faster transient response and lower tracking error, which prove the validity of BA-TFFPD.


I. INTRODUCTION
DELTA robot and SCARA robots dedicate to pick-andplace tasks in the industrial production line [1].On the one hand, the translational DELTA robot, whose mobile platform can only realize translations in three directions, is difficult to meet more complex task with object realignment.DELTA robot with an additional actuator on the end-effector or the fourth RPUR(R, P and U stand for revolute, prismatic and universal joints, respectively) limb to connect the centers of the base and mobile platform still has some drawbacks as revealed in [2].On the other hand, the serial SCARA robots cannot work in high speed due to its large dynamic inertia, though it can produce three independent translations and one rotation around an axis of fixed direction, namely, Schönflies motion.Thanks to its high speed, low inertia and high accuracy, parallel Schönflies motion manipulator very popular in industrial applications.
Up to date, many types of Schönflies motion generators have been proposed, such as H4 [3], I4 [4], Par4 [5].And amongst others [6]- [9], the Adept Quattro robot and the Veloce.Robot are famous for their commercial application.Compared to the parallel manipulators with only single platform [10] [11], the four-limb robots, whose mobile platform consist of two sub-platforms, can realize more large range of rotation motion, thanks to well-configuration of subplatforms.Moreover, some pick-and-place tasks of those objects with irregular shapes require full-circle rotation.Rotational motion can be realized by the relative movement in parallel between the two sub-platforms of the robot, such as H4, Quattro and Par4.As usual, pulley-belt mechanism [4], rack-pinion mechanism [5], gears and screw mechanism [12] can be adopted to enhance the rotational capability of manipulator.For this article, helix joint is employed to enhance the rotational capability of end effector.
To apply the manipulator into manufacture, designing a control system, with high tracking accuracy and well effected error compensation, will be a crucial work.For this goal, a number of control laws are designed by researchers.Modelfree control technique is usually employed in trajectory tracking task.PID as a classical model-free controller is used widely in industrial manufacturing thanks its simple structure.In fact, conventional PID cannot adapt to variation of external disturbance and un-modeled uncertainty.So other type of PID controllers, such as fractional-order PID [13]- [18], is investigated.W. A. Shutnan and T. Y. Abdalla [19] presented the fractional-order PID controller for path tracking task of robot manipulator to obtain better performance than classical PID controller.J. Viola and L. Angel [20] used fractional-order PID controller with the feedback linearization technique to design the tracking system of parallel manipulator, which get better tracking performance than integer order PID.On the other hand, combining other algorithm also can enhance the tracking performance of PID controller [21]- [23].V. Nguyen et al. [24] proposed radial basic function neural networks based PID (APID-RBFNs), which is used in tracking task of two degree of freedom parallel manipulator.P. C. Sau [25] compared fuzzy fractional-order PID (FFOPID) controller and fractionalorder PID (FOPID) for the control system of three-arm manipulator.And the better tracking performance can be obtained by FFOPID.
Differ from model-free control technique, model-based control technique needs dynamic model of manipulator to design the controller.As a classical method, computed torque control (CTC) is widely employed in industrial robot and parallel manipulator [26].But classical CTC has many Journal of Robotics and Control (JRC) ISSN: 2715-5072 528 Z. Lin, Dynamic Modeling and Torque Feedforward based Optimal Fuzzy PD control of a High-Speed Parallel Manipulator drawbacks, such as higher computational cost and the difficulty of getting precise dynamic model [27][28].For those reason, many modified algorithms.J. Viola and L. Angel [29] based computed torque control strategy design a fraction-order PID controller for two degree of freedom manipulator to reduce the error generated during tracking task.M. Alhaddad et al. [30] presented an adaptive LQ-based computed torque controller (LQ-CTC), which can improve the accuracy of robotic manipulator carrying unknown load.
An excellent controller needs well parameter tuning.And a lot of metaheuristic parameter searching algorithm, such as particle swarm optimization (PSO) algorithm [31], bat algorithm (BA) [32], grey wolf optimization algorithm (GWO) [34], genetic algorithms (GA) [35][36] and cuckoo search (CS) algorithm [37] are adopted in optimization of parameter of controller for its powerful performance for tough optimization problems.T. M. Ahmed et al. [16] used PSO to optimize the parameters of fractional-order PID controller.Liu et al. [38] designed fuzzy controller, whose bottom width of membership functions were tuning by improved PSO, to adjust PID gains.And the result of its simulation can significantly reduce tracking error and contour error of the three-axis machine tool.F. Liu et al. [39] use bat algorithm to optimize the center and width vectors of membership functions of fuzzy control system adopted in autonomous mobile robots and lead a better tracking performance.
In this paper, a parallel manipulator with SCARA motion [8] is presented together with the dynamic modeling and control design.Architecture of the manipulator is introduced at first.Then, principle of virtual work is adopted for dynamic modeling and evaluated by simulating in MATLAB/Simulink.For the next part, the torque feedforward-based PD controller is proposed.At the same time, to fix the issue of parameters tuning, fuzzy controller, whose bottom width of membership functions are optimized by bat algorithm, is adopted to adjust PD gains in real-time.For simulation, a numerical manipulator model is built and the normal trajectory is used to evaluate the performance of trajectory tracking.Finally, the conclusion will be summarized by compared performance of the bat algorithmbased torque feedforward fuzzy PD (BA-TFFPD) controller to computed torque controller (CTC) and torque feedforward PD (TFPD) controller., can be split into two subparts, namely, the upper and lower platforms that are connected by helical pair to convert the relative translation along vertical axis of two subparts into rotational movement about the axis.The rotational capability can be enhanced by selecting an appropriate screw lead that is denoted as h.

II. ROBOT ARCHITECTURE
As shown in Fig. 1(a), the origin of global coordinate frame (x, y, z) is located at the geometric center of the base frame.The z-axis is in vertical direction, and x-axis is parallel to segment A1A2 that is the line between actuated joint 1 and 2. The moving coordinate frame (X, Y, Z) is located at geometric center of the lower platform and the Z-axis and Xaxis are also parallel to z and x-axis, respectively.Moreover, i, j and k are the unit vectors of x, y and z-axis, respectively.
The ith limb, depicted in Fig. 2, consists of inner arm and outer arm.Ai stand for the center of actuated joint.Bi and Ci represent the center of two spherical joints at two ends.In addition, length of inner and outer arms can be written in b and l respectively.The axis of rotation of the ith actuated joint is parallel to unit vector ui = Ri k, where Ri stands for the rotation matrix from global coordinate frame to ith actuated joint.Here, the principle of virtual work is adopted to model the dynamic model of the parallel manipulator.

A. Kinematic Jacobian
The closed-form solution [9] of the robot geometry has been extensively studied in the literature, which will be skipped in this paper.For each limb, depicted in Fig. 2, geometric constraints as below must be satisfied.
where, ci and bi are the position vectors in the global coordinate frame oxyz.
Differentiating (1) with respect to time yields where, J  and J  are the forward and backward Jacobian matrices, respectively.
As long as the forward kinematic Jacobian is nonsingular, the kinematic Jacobian matrix is obtained as: Because of the mobile platform consists of two subplatforms, the upper platform can translate in vertical direction with respect to the lower one, which introduces different velocities between the end-effector and subplatforms.Besides, the Jacobian matrix between upper platform and actuated joints is also different from that of the lower one.
The velocities of the upper and lower platforms can be expressed as 1 1 where up  , low  and  represent the velocity vector of upper platform, lower platform and end-effector, respectively.And the Jacobian matrices of the two sub-platforms can be written below Sequentially, the kinematic Jacobian matrices of the two sub-platforms are obtained as Finally, the relationship between the velocities of subplatforms and actuated joints are derived as

B. Dynamic Model
Before modeling, some assumptions are made, where the torque caused by rod inertia of the outer arm should be small enough to be ignored.Moreover, their mass is simplified as two lumped masses located at the two ends, as shown in Fig. 3. where is the torques vector exerted on the actuated joints.
for which the mass properties can be listed in TABLE I.
Differentiating ( 12) with respect to time yields Analogue to the time differential equations of motion, small-amplitude displacements can deduce as following ,, Combing ( 13) -( 19), ( 13) can be rewritten as After rearranging (21), the simplified dynamic model can be obtained as For the simulation, as shown in Fig. 4, the trajectory with horizontal displacement of 305 mm, vertical displacement of 25mm and rotational motion of 360 degree is adopted.Then, 4-5-6-7 polynomial is used in trajectory design.Fig. 5 depicts the displacement, velocity and acceleration of x, y, z-axis and rotational motion.The comparison between the torque of analytical model (solid line) and simulation model (dotted line) is shown in Fig. 6, from which it can be seen that the computation accuracy of the simplified model is very close to that of the complete model, despite the acceptable errors.From Fig. 7, which show the absolute torque error of two models, it can be seem that the max error of 0.8 N*m for joint 1, 1.7 N*m for joint 2, 0.9 N*m for joint 3 and 2.3 N*m for joint 4.And due to the inertia of the rods are ignored, those max errors occur at the point of max acceleration of each joint.

IV. CONTROLLER DESIGN
In this section, bat algorithm-based torque feedforward PD (BA-TFFPD) controller will be introduced.An independent PD controller for each will be employed for main controller.Then, the torque calculated offline will be introduced into the PD control system to improve the tracking performance.Moreover, in order to adapt to time-varying disturbance, fuzzy logic will be used to tune the gains of PD controller in real-time.And, bat algorithm also will be used to optimize bottom widths of membership functions of the fuzzy controller.

A. Torque feedforward based PD control
The complete dynamic model should be the sum of simplified model and un-modeled uncertainty () ft, namely, ( ) The tracking error can be defined as Sequentially, ignoring the coupling of each joints, it can be designed an independent PD controller for each joint.([ ]) ([ ]) where Kp and Kd are the proportional and derivative gains matrices, respectively.
Introduce the torque calculated offline, the control law can be summarized as Where, From (32), obviously, the tracking error e can converge to zero in finite time by choose appropriate Kp and Kd.And the method of trial and error can be used in gains selection.

B. Parameter tuning strategy of PD controller
Tracking error cannot maintain in a low level, when the gains of PD controller are fixed.Fuzzy logic is effective way to adaptively tune parameter online.But, it is a hard work to design fuzzy controller appropriately, in which bottom widths should be designed according to the experiential knowledge summarized by proficient or trial and error method.However, for trial and error method, it is not a sensible way to spend lot of time on these repetitive work that is more suitable for computer.Therefore, optimization algorithm, such as bat algorithm (BA), particle swarm optimization (PSO) algorithm and cuckoo search (CS) algorithm, can be considered to employ in parameter tuning.For those reasons above, fuzzy logic based parameter tuning strategy will be presented in this part.At the same time, bat algorithm will be employed to optimize bottom widths of the fuzzy controller.
During trajectory tracking task, gains of PD controller should be adjusted in real-time to adapt to different situations.The strategy of parameters adjustment can be summarize as ([ ]) Where p K , d K defined in ( 29) are base gain and set to be constant, while gains are composite gains p K  and d K  , which can be adjusted in real-time with tracking error changes.Generally, four fuzzy controllers are needed to tune the parameter of PD controller for each joint.In fact, because of symmetry of the manipulator, only two fuzzy controllers need to design for four joints: a fuzzy controller for joint 1 and 3 and another controller for joint 2 and 4. Then we will introduce the design process of one among them in this part.
For fuzzy controller, as shown in Fig. 8, it can be summarizing as 4 parts: input fuzzification, fuzzy rule base, inference engine and output defuzzification.And the design process can be summarized as follow: First of all, for each joint, it needs a fuzzy controller to tuning both adjustable gains Then, before the inference engine, inputs of the controller, which have crisp value, need to transform to membership function expression, which is call fuzzification.And, in this paper, the triangle membership function, which can be expressed in (37), is adopted for input and output variables.
x q q x q q q q x q q x q q x q x q q q q q x Where, x stands for those input and output variables, it can be e , e , p k  and d k  .And q and q  represent vertex and half the length of the base of the triangle, respectively.For example, as depicted in Fig. 9, q = 0 and 0.1 q = for fuzzy linguistic terms "Z ".
For the next step, fuzzy rules, the most important process, are designed.Generally, it should set different rules for different control system.For the PD controller of ith joint, four situations, as shown in Fig. 10, possibly meet during tracking.And the laws of gain tuning can be summarized as follow case:  II.What's more, every rule can be written as "IF-THEN" expression.For instance, Then, for outputs, according to centroid method, it can be expressed as Where () Symbol of "  " denote minimum operator.C. Bat algorithm based optimal parameters of fuzzy controller Bat algorithm (BA), was presented by Yang [40], and was usually used for parameters optimization thanks to that it combines the advantages of existing algorithms.
For BA based optimization of parameters, the update rules of characteristic of virtual bats can be denoted as min max min () Where At last, optimization object of the whole system is designed as Where, symbol " 2 y " denote the 2-Norm of vector y.According to literature [32][33] [40], Pseudo code of BA can be summarized in Fig. 11.

V. SIMULATION RESULT
The control design is numerically illustrated with the torque calculating by simplified model, along the test trajectory, as displayed in Fig. 4. The corresponding designed control scheme is depicted in Fig. 12, in which the saturation can limit the torques less than its maximum value of 100 N*m.
In this paper, classical computed torque control (CTC) method is compared.According to [20], the control law of CTC can be summarized as: Where, For the simulation, ( , , ) ft  is a function to simulate un-modeled uncertainty () ft in (26), which include friction and external disturbance and so on.Then, function ( , , )  ft  in this article can be described by Initialize the bat population [ ( ,1), (   And according to characteristics of normal pick-andplace trajectory, as shown in Fig. 5, and (50), the un-modeled uncertainty can be displayed in Fig. 13.About the setting of the PD controller, base gains .In order to obtain a good optimization result, the parameters of bat algorithm can be set referenced to [32]

Fig. 1 (
Fig. 1(a) depicts the simplified CAD model of the parallel robot under study.The robot has four limbs, to connect the base frame and mobile platform, of which each limb consists of an inner arm and an outer arm (parallelogram, a.k.a.,  joint).The mobile platform, depicted in Fig. 1(b), can be split into two subparts, namely, the upper and lower platforms that are connected by helical pair to convert the relative translation along vertical axis of two subparts into rotational movement about the axis.The rotational capability can be enhanced by selecting an appropriate screw lead that is denoted as h.

Fig. 3 .F
Fig. 3. Simplification of the rod.According to the Principle of virtual work, sum of the work done by the external and internal forces should to be zero.The external forces include actuating torque  and the gravities b M , , p up M and of actuated joints and sub-platforms, respectively.Moreover, of the dynamic modelThe dynamic model described by(22) can evaluated by Simscape module of MATLAB/Simulink.Through comparing the actuating torques calculating by simplified model and simulated by Simscape model built completely and without any simplification, the difference between simplified model and complete model can be observed for assessment.

Fig. 6 .Fig. 7 .
Fig. 6.Actuated torque of each joint to(30) and (31), the system equation can be obtained are adjustable gains produced by fuzzy controllers.The sum of base gains and adjustable ISSN: 2715-5072 532 Z. Lin, Dynamic Modeling and Torque Feedforward based Optimal Fuzzy PD control of a High-Speed Parallel Manipulator considered as inputs of the fuzzy controller, because they can reflect the tracking performance.Then, adjustable gain pi k  and di k  are outputs of fuzzy controller.The fuzzy set of input and output variables are defined as   A B D NB NM NS Z PS PM PB = A , B are the fuzzy sets of input variables e and e , respectively.C , D are the fuzzy sets of output variables p k  and d k  .And NB: negative big; NM: negative medium; NS: negative small; Z: zero; PB: positive big; PM: positive medium; PS: positive small;

Fig. 9 (
a) and (b) show the degree of membership about different value of input and output variables.

Case 1 (
point A): when i e is big and i e is in the direction close to zero, we should increase pi k and decrease di k to enforce tracking error close to zero in a short time.Case 2 (point B): when i e is small and i e is in the direction close to zero, decrease pi k and increase di k to enforce tracking error close to zero in a slow speed.Case 3 (point C): when i e is small and i e is in the direction away from zero, increase pi k and decrease di k to enforce tracking error return to zero in a short time.Case 4 (point D): when i e is big and i e is in the direction away from zero, increase pi k and decrease di k to enforce tracking error return to zero in a short time.Based on above tuning law, fuzzy rule of the fuzzy controllers, whose output are

D
are subsets of fuzzy set A , B , C and D .The last step is that according to the fuzzy rules to infer and defuzzification the output variables

Fig. 11 .
Fig. 11.Pseudo code of the bat algorithm

4 (
[33][40], i.e. use i = 20 bats, = =0.9 , at 10 -4 s. then, set initial condition of the manipulator model as(0)   error.In the case of un-modeled uncertainty existing, Fig.14shows the tracking performance of each joints under bat algorithm-based torque feedforward fuzzy PD (BA-TFFPD) controller, torque feedforward PD (TFPD) controller and computed torqued controller (CTC).And angular error of each joints is shown in Fig.15.TABLE III is the mean tracking error of each joints during tracking task.Compared with CTC, as shown in Fig.14, BA-TFFPD can reduce the tracking error in joint space (e.g. the mean tracking error of TFFPD of joint 1 , even if un-modeled uncertainty is existing.What's more, it can be seem in Fig.

TABLE I .
(17)MASS PROPERTIES OF THE ROBOT IN (14)-(17) Combine fuzzy logic and bat algorithm, it is easy to select bottom widths Li of membership functions of fuzzy inputs and outputs as optimization variable x .According to Fig.9,