Collaborative Position Control of Pantograph Robot Using Particle Swarm Optimization

This article presents the design and real-time implementation of an optimal collaborative approach to obtain the desired trajectory tracking of two Degree of Freedom (DOF) pantograph end effector position. The proposed controller constructively synergizes the Proportional Integral Derivative (PID) and Linear Quadratic Regulator (LQR) by taking their weighted sum. Particle Swarm Optimization (PSO) algorithm is proposed to optimally tune the gains of PID, weighting matrices of LQR, and their ratio of contributions. Initially, the PID and LQR controller parameters are optimally tuned using PSO. In order to enhance the control effort and to provide more optimal performance, the weightages of each controller are optimally tuned and are kept constant. The collaborative position control strategy is tested against the PID and the LQR controllers via hardware in loop trials on a robotic manipulator. Experimental results are provided to validate the accurate trajectory tracking of the proposed controller. Results demonstrate that the optimal combination renders a significant improvement of 10% in steady-state response and about 37% in transient response over the PID and LQR schemes.


INTRODUCTION
Robotic manipulators are widely used in assembly lines for mounting, transporting, cutting, production, and welding, etc [1,2]. Pantograph robotic manipulators are the most common types of robots used in industrial applications [3]. A pantograph is a mechanical linkage structure connected in a parallelogram-like manner. An accurate end effector's position of a manipulator is critical for high-performance robotic applications [4]. Control of a robotic manipulator deals with the problem of formulating the joint angles required to move an end-effector to follow a certain specified position trajectory. The kinematics of a manipulator and dynamics of actuators are derived to synthesize and physically realize the controller based on a certain hardware platform.
Precise position control of a robotic manipulator has received great attention of the researchers and engineers [5][6][7][8]. Most of these control algorithms are complex and do not provide generally an optimal performance. However, Linear Quadratic Regulator (LQR) and Proportional Integral Derivative (PID) controllers are still vastly used control strategies [9]. The LQR control law cannot com-pensate modeling errors [10,11]. In addition, the state and input weighting matrices define the control performance and must be adjusted optimally to achieve desired objectives. The tuning of a PID controller is practically intuitive. However, it does not guarantee the desired performance [12]. A properly tuned PID controller eradicates overshoot and steady-state error caused by transients in the response [13].
Tremendous research is being done to improve the position performance of robotic manipulators by combining PID and LQR controllers [14][15][16]. In [14], the gains of individual PID and LQR controllers are computed intuitively which is not an optimal way. Moreover, the combination of the gains of the two control laws is also nonoptimal. Research work reported in [15] presents a FCPC scheme to control longitudinal dynamics of an aerial vehicle. However, the gains and weightage are not optimal. Similarly, in [16], the gains of PID and LQR controllers have not been optimized. However, fuzzy rules have been used to tune the weightages ξ 1 and ξ 2 of both the controllers. In contrast, we have used a single optimal weight h, thus reducing the optimization complexity. The perfor-mance cooperation can be further enhanced by optimizing the gains of the individual controller. Numerous optimization algorithms are reported in the literature. Metaheuristic Genetic Algorithm (GA) is used in many optimization problems [17][18][19]. The algorithm takes a very long time to search global best from a population of points rather than a single point [20]. Moreover, the conventional optimization tools intent to minimize the cost function only and do not contemplate some control objectives like reducing settling time, overshoot, and steady-state error [21]. In [19], the authors used two reactive evolutionary and four swarm-based algorithms including GA, differential evolution, Bat, hybrid Bat, cuckoo search, and Particle Swarm Optimization (PSO) to tune the PID control gains. Among these algorithm, PSO is the best candidate for optimization under the conditions of small population size and less iterations. The PSO method is addressed in the present research because of its ease in computation and quick convergence to the global minimum [22,23]. Motivated by the abovementioned discussion, the main contributions of this paper are: 1) The PSO algorithm is used to optimally tune the design gains of PID and weighting matrices of LQR control schemes. In addition, their experimental validations are presented.
2) To overcome the disadvantages and to get the benefits offered by both control efforts, the weightage is optimally tuned via PSO to synthesize more efficient control algorithm. We design and implement a Fixed-weighted Collaborative Position Controller (FCPC) on a real-time 2-DOF pantograph robotic manipulator to improve its response in terms of settling time, steady-state error, and overshoot.
The remaining paper is organized in the following sections: Mathematical model of a 2-DOF robot manipulator is derived in Section 2. Section 3 presents the theoretical background of PID, LQR and the proposed FCPC control techniques. Parameter optimization is discussed in Section 4. The laboratory setup is described in Section 5. The experimental results and performance analysis of the proposed controller are presented in Section 6. Finally, concluding remarks are given in Section 7.

MATHEMATICAL MODEL
The present study considers a 2-DOF pantograph robot by Quanser, whose mathematical model is derived analytically. The robotic hardware benchmark consists of two identical rotary servo units (SRV02) connected together with a four-bar linkage [24]. The electrical circuit schematic including gears train of SRV02 is shown in Fig. 1. The load shaft position and speed dynamics for each SRV02 actuator are given bẏ where A = R m (B m η g k 2 g + B l ) + η m k m k t η g k 2 g . The electromechanical parameters used in this research and their nominal values are listed in Table 1, see [24,25]. In [24], these parameters are experimentally verified using bump test and frequency response methods. In practical, these values may be slightly uncertain from the real robot. However, these uncertainties are not serious and brings only a small steady-state error [26], which we have ignored during the optimization process in section 4.

Forward Kinematics
The forward kinematics computes the cartesian coordinates of Quanser 2-DOF robot end effector based on the actuated angles θ A and θ B of servos A and B respectively. The 2-DOF manipulator consists of a four bar linkage structure having equal length, denoted by L b . The forward kinematics of the robotic manipulator is shown in Fig. 2, where the end effector's position is denoted by joint E. Looking at the top of the manipulator, the anticlockwise rotation of the actuator's angles is taken as positive. The cartesian coordinates of joints D and C are given by Referring to Fig. 2, the distance between point D and C can be computed by applying Pythagoras theorem. The triangle CDE is an isosceles and all its sides are known. So, the angles α 1 and α 2 at vertex D can be expressed as Using trigonometry, forward kinematics of end effector's position can be written as,

Inverse Kinematics
The inverse kinematics (IK) finds the actuated angles of two SRV02 plants from the cartesian coordinates of the end effector of the manipulator. The known quantities in IK are E x and E y coordinates of the location E, as illustrated in Fig. 3. Considering triangle △ABE, the sides represented by m and n can be expressed in terms of position of the end effector as Using trigonometry, the angles ϕ A and ϕ A can be given by By using angles sum theorem for triangles and solving for Finally, the servo actuators angles can be calculated as

CONTROL STRATEGIES
The theoretical background of conventional PID, LQR and FCPC control schemes are discussed as follows:

PID controller
Among the linear control strategies, PID is a simple model-free control technique and is commonly used because of its simplicity [27]. The control algorithm comprises of three modes, i.e., proportional, differential, and integral. In this research, PID control is designed and is implemented on hardware (see section 6) to validate its performance for the position control of the SRV02 system. The control law is given by (17) where k p , k d and k i are PID tuning gains and (θ e ) is the position tracking error between measured angle θ and the desire angle (θ d ) of SRV02 link. Designing a PID controller appears to be conceptually intuitive since the designed gains are repeatedly adjusted and are then incorporated into real-time Simulink model to achieve the desired performance.

Linear quadratic regulator
Among the modern optimal control techniques, LQR is widely used [28]. The control algorithm uses the dynamic model with complete information of the system to minimize quadratic performance index given by (18) and calculates the optimal gains to enhance the system performance.
The weight matrices Q and R are tuning variables and penalize states x and control input u respectively. The feedback control law can be designed as where k denotes optimal feedback gains and is given by where P = P T ≥ 0 is a solution of algebraic Riccatti equation i.e.

Collaborative position controller
The fixed-weighted collaborative position controller (FCPC) synthesizes a synergistic control scheme utilizing the designed PID and LQR controllers [14][15][16]. To overcome the drawbacks of each controller and to utilize their benefits, both the controllers are linearly augmented. Block diagram of the proposed control scheme is shown in Fig. 4. The associated control law in continuous time can be defined as, where h ∈ [0, 1] characterizes weights of PID and LQR. The weightage parameter in FCPC is tuned using PSO explained in section 4.

PARTICLE SWARM OPTIMIZATION
The PSO algorithm is a population-based optimization technique [29]. A population of random particles is initially selected for optimal tuning of the gains/parameters. It then searches the entire space to converge to the global best-fit solution. Each particle has a position (X i ) and velocity (V i ) associated with it. The relationship used to update the position and velocity of a particle are given as where m 1 and m 2 are cognitive coefficients, r 1 and r 2 are random real numbers and w is the inertia weight. The first term of (24) denotes the current motion of a particle, while the second term is called as 'cognitive term' which is the difference between current position of a particle and its best local position (P i ). The third term is known as 'social term' which is the difference between current position of a particle and global best swarm position (P g ). In this work, the PID gains, LQR weighting matrices, and weightage parameter in FCPC are optimally tuned. After initialization, the PSO algorithm iteratively computes and stores the fitness values of each particle using (25) (25) where OS denotes overshoot, T s is settling time, θ e is position tracking error and v m is the applied voltage. The optimization problem is to minimize the fitness function (25) to ensure less overshoot and fast response of the system. The integral-squared-error (ISE) and integral-squared input voltage performance indices are used to achieve the optimal control effort. For each particle, the fitness value is compared with the existing best objective value also called as 'local best' (P i ). The particle with highest fitness value is updated as new P i and is chosen as 'global best' (P g ) [30]. The optimizer inertia weight (ω j ) given in (26) decreases from 1 to 0 in each iteration in order to explore the best solution in search space.
where j and j max are current iteration and maximum defined iteration respectively, ω max and ω min are set at 0.902 and 0.394 respectively. The flowchart of the PSO algorithm is shown in Fig. 5. A population of 100 particle candidates is taken for optimization of the fitness function. For the PID and LQR schemes, each particle has a set of three members X i = [k p , k i , k d ] and X i = [q 11 , q 22 , r] respectively. It means that the particles fly in a three-dimensional space and searches for the optimal values of the gains. For the proposed FCPC, each particle has one member h and moves along a line. During each iteration, as the particles assume new positions X i , the gains values are used to run the 2DOF and the fitness is evaluated according to (25). The local best of each particle is saved as P i , and the particle with minimum fitness value is added as the global best variable P g . To optimally tune the control laws and to minimize the fitness function, 20 iterations have been performed as shown in Fig. 6. It can be seen that the PID controller takes less iterations to reach its optimum solution as compared to the LQR and FCPC schemes. Conversely, the optimization process of the FCPC results in smaller fitness value. Therefore, PSO-FCPC is the best option to control 2-DOF end effector. The average elapsed time is recorded as is the elapsed time for each iteration. The average time is same for tuning the inputs (17), (19) and (22) and is given as T a = 9.02 sec. The optimized gain values of the proposed control schemes, their ranges, and the corresponding values of the fitness function are presented in Table 2.

EXPERIMENTAL SETUP
The experimental test bench centered on Quanser 2-DOF robotic manipulator is presented in Fig. 7.  The hardware setup consists of two SRV02 plants connected together via four link bars having same lengths and a Q3 Control PaQ-FW data acquisition device interfaced with MATLAB/Simulink running on a PC as shown in Fig. 8. The whole setup is implemented via hardware in loop (HIL) experiments. SRV02 comprises of a DC motor with planetary gearbox, motor pinion gears, load gears, potentiometer backlash gears and ball bearing block. The plant comes with a built-in potentiometer and a high-resolution encoder providing 4096 counts per revolution to measure the position of the load link. The Q3 control PaQ-FW is an advanced HIL control board that can be easily interfaced with MATLAB or LABVIEW with an available 6pin FireWire (IEEE 1394) input to perform a wide range of laboratory experiments. The control board comes with a built-in power amplifier and a data acquisition card. It supports 16-bit three encoder input channels numbered from 0-2. The two encoder channels, i.e., channels 0 and 1 are configured in HIL initialized block to read angular position data of both SRV02 plants. In addition, three PWM outputs channels are available in which two of them are used to drive the SRV02 motors.

TESTS AND RESULTS
To characterize the performance of the proposed control laws, real-time tests have been performed using the QUARC library installed in MATLAB 2009a, running on a computer system with the following specifications; Dell OptiPlex 990, Intel(R) Core(TM) i3-2120 CPU @ 3.30GHz, 32-bit Operating System. The resulting responses are graphically visualized and recorded for two different cases.
Case 1: In this case, the experiment is conducted to investigate the tracking performance of the manipulator's end effector for a square reference signal using the designed control laws. The control parameters are taken from Table 2. The end effector of the robotic manipulator is initially set at home position i.e., (E x0 , E y0 ) = (L b , L b ) assuming the joint angles are at the origin. A square wave signal is applied as a position reference trajectory.   9 show experimental results of the proposed optimal control laws in which the position tracking performance of SRV02 actuators is presented. The results demonstrate that using FCPC approach, the actuator load shaft tracks the desired position with a reasonable steadystate as well as transient performance. Based on the derived forward kinematics, the position of the manipulator's end effector on the x-axis and y-axis is depicted in Fig. 10a and Fig. 10b respectively. It is clear that the PID controller gives poor transient performance. The LQR controller exhibits good transient behavior but with poor steady-state response due to lack of integral action. As evident from the results, the performance with FCPC is superior than with PID controller in terms of transient response, while the achieved steady-state response with FCPS is better than obtained with LQR control scheme.   11 demonstrates the two-dimensional path drawn by the manipulator's end effector. It can be observed that LQR exhibits relatively fast response with an overshoot of 2.025%. FCPC scheme surpasses the PID and LQR responses. For each of the three control schemes, experimental test results are given in Table 3. The hardware results clearly manifest that the PSO based FSPC control strategy is superior in tracking than other two optimal control schemes.  Fig. 11. Manipulator tracking performance in Cartesian plane.
Case 2: In this case, we considered the desired endeffector position as E x d = L b +0.5 sin(ωt) with ω = 0.471. The y-component is chosen as E y d = 0 for t < 0.5 sec and E y d = L b + 0.5 sin(ωt − 0.5) for t ≥ 0.5 sec as shown in Fig. 13. The manipulator is set to the same initial position as that of case 1. The actuator's tracking performance is illustrated in Fig. 12. From the rotary servos data, the manipulator's positions are calculated using (5) and (6) and are plotted in Fig. 13.     To analyze the results in case 1 and case 2, the performance index Integral Time Absolute Error (ITAE) i.e., ITAE = t|e i |dt values for i = x; y are calculated to characterize the tracking performance. Based on the results in Fig. 15, it can be concluded that the optimally tuned FCPC in (22) demonstrates superior tracking performance in comparison to the optimal PID and LQR counterparts expressed in (17) and (19), respectively.

CONCLUSION
This research models Quanser 2-DOF robotic manipulator and addresses its behavior including the actuator dynamics to design optimal control approaches. Modeling of the robot involves derivation of dynamics of rotary actuators and manipulator kinematics. The optimal control techniques under study include PID, LQR and FCPC. The experimental validation of proposed control techniques has been carried out on a robotic manipulator consisting of four bar linkages and two rotary servo plants. PID approach resulted poor transient performance, the LQR overcomes the shortcoming of PID but demonstrates unwanted overshoots. The proposed FCPC scheme synthesizes a control law that collaborates the individual control efforts provided independently by PID and LQR. The actual position quickly converges to the desired trajectory demonstrating 10% improvement in steady-state error and about 37% in transient response as compared to the individual efforts of PID and LQR. In future, the optimal weighted combination of model-based nonlinear con-trol schemes can be tested to control the manipulator end effector position using hybrid PSO and Grey Wolf Optimizer. Also, it is anticipated in near future to evaluate the performance improvement in the presence of disturbances.