Research on Trajectory Tracking Method of Redundant Manipulator Based on
PSO Algorithm Optimization

Aiming at the problem that the trajectory tracking performance of redundant manipulator corresponding to the target position is difficult to optimize, the trajectory tracking method of redundant manipulator based on PSO algorithm optimization is studied. The kinematic diagram of redundant manipulator is created, to derive the equation of motion trajectory of redundant manipulator end. Pseudo inverse Jacobi matrix is used to solve the problem of manipulator redundancy. Based on the tracking ellipse of redundant manipulator, the tracking shape of redundant manipulator is determined with the overall tracking index as the second index, and the optimization method of tracking index is proposed. The redundant manipulator contour is located by active contour model, on this basis, combined with particle swarm optimization algorithm, the point coordinates on the circumference with the relevant joint point as the center and joint length as the radius are selected as the algorithm particles for iteration, and the optimal tracking results of the overall redundant manipulator trajectory are obtained. The experimental results show that under the proposed method, the tracking error of the redundant manipulator is low, and the error jump range is small. It shows that this method has high tracking accuracy and reliability.


Introduction
Manipulator can imitate some movement functions of hand and arm, and it is an automatic operation device for grabbing and carrying objects or operating tools according to fixed procedures [1]. Manipulator is the earliest industrial robot and the earliest modern robot. It can replace human's heavy labor to realize the mechanization and automation of production, and can operate in harmful environment to protect personal safety. Therefore, it is widely used in mechanical manufacturing, metallurgy, electronics, light industry, atomic energy and other departments. Manipulator trajectory planning is an important problem of robot control [2]. The trajectory of the end effecter can be generated by the position sequence of the end effecter of the manipulator motion planning. The computer programming can only determine and store the limited position. The robot motion controller can calculate and move the manipulator in two discrete positions, making the terminal manipulator draw a continuous path. The optimization of robot trajectory means to determine the number of intermediate positions and their optimal combination [3]. Therefore, a large number of optional trajectory needs to be optimized. Because of the complex relationship between the robot's arm configuration sequence and the trajectory of the end effecter, the trajectory generation becomes a highly nonlinear optimization problem in a huge state space.
In recent decades, redundant manipulators have been used in various industries, such as welding, packaging and grinding. The redundant manipulator itself has more degrees of freedom [4], that is, redundancy, than the required degrees of freedom to complete the arm end tasks (such as trajectory tracking). The robot system operates in redundant condition, which makes the performance index of trajectory difficult to be optimized. Of course, the calculation difficulty caused by redundancy can be eliminated by locking redundant members. However, such artificial simplification will lead to the performance degradation of the system, especially the robot flexibility provided by the designer can not be utilized, thus reducing the practicability of the system. Many researchers in the field of kinematics are studying how to use redundancy to solve the problems of trajectory tracking and avoiding odd and irregular shapes.
Xiao et al. [5] proposed a new kind of GA-RBF neural network closed-loop adaptive control method, which uses RBF neural network to approximate and compensate the model error and external disturbance of the system, realizes the trajectory tracking control of the manipulator based on the calculation moment method, and optimizes the weights of RBF neural network on line by using genetic algorithm (GA), To ensure the stability of the manipulator control system in a shorter time, but the control speed of the method is slow. Sun et al. [6] proposed a visual interactive hand tracking algorithm based on the fusion of infrared depth information. Using the particle filter tracking algorithm based on the local binary pattern feature of gradient direction, the local and global feature histogram description was established to realize hand tracking. Aiming at the problem of particle shortage, the infrared depth information was used and the artificial bee colony algorithm based on swarm intelligence was introduced, the current observation information is fused in the sampling and updating phase of particle prediction, which can efficiently search and optimize the target, reduce the attenuation degree of particle set, and improve the accuracy of state estimation. However, the tracking error of this method is large. Wei et al. [7] used the improved neural network PID controller to create the plane sketch of the manipulator's motion mechanism, deduced the geometric relationship of the manipulator's end actuator's motion, and described the working principle of servo valve control, the pressure and flow control equations are given. The on-line control flow chart of the hydraulic drive control of the manipulator is given by PID controller. The experimental results show that the tracking accuracy of the manipulator can be improved by using the improved neural network PID control, but the control speed is slow.
Particle swarm optimization (PSO) is a new swarm intelligence optimization algorithm, which originates from the research of the motion behavior of bird and fish swarm [8]. It is a new branch of evolutionary computing. PSO has been widely used in function optimization, neural network training, combination optimization, robot path planning and other fields, and has achieved good results [9]. Although the particle swarm optimization algorithm has developed for nearly ten years, it is not mature in both theoretical analysis and practical application, and there are a lot of problems worth studying [10]. This paper focuses on PSO algorithm and its application, and studies how to improve the performance of traditional PSO algorithm and its application performance in redundant manipulator trajectory tracking [11]. On this basis, a trajectory tracking method of redundant manipulator based on PSO algorithm is proposed, and how to use PSO algorithm to optimize the trajectory of redundant manipulator is discussed: taking the study of overall tracking index as the second index, the shape of redundant manipulator is proposed, and combining with PSO algorithm, a method of overall tracking optimization is given, that is, the point coordinates on the circumference with the relevant joint point as the center and the joint length as the radius are taken as the particles for iterative optimization. In this way, the arm end of the redundant manipulator not only completes the given task, but also keeps high maneuverability of the whole arm at all times.

Kinetics of Redundant Manipulator
Taking the redundant manipulator of three joint mechanism as an example, it is composed of two rotation joints and one translation joint, each joint is driven by the hydraulic cylinder connected to the servo valve [12], and the motion diagram of redundant manipulator is created, as shown in Fig. 1.
According to the coordinate system in Fig. 1, the formula of the motion trajectory of redundant manipulator end can be deduced as follows: where, x 1p and x 2p are the coordinate positions of point P; L 1 is the length of joint 1; L 2 is the length from the rotation point of joint 2 to the end; q 1 and q 2 are the angular displacement of joint 1 and joint 2; q 3 is the kinematic displacement of joint 3.
The kinematic and derivative formulas of redundant manipulator including n joints are as follows: where, x 2 R m n > m ð Þ is the coordinates of the end points with m degrees of freedom; x 0 is the tracking speed; q 2 R n is the joint position; q 0 is the joint speed; J q ð Þ 2 R mÂn is the Jacobi matrix, defined as: The Jacobi matrix of kinematics is as follows: For the redundant manipulator (n ¼ m) without repetition, the relationship between the joint angular velocity and the motion track velocity can be deduced by combining the above formula as follows: For redundant manipulators, Jacobi matrix is non square and irreversible [13]. Therefore, the pseudo inverse matrix of Jacobi matrix is: Formula (8) defines the pseudo inverse Jacobi matrix, which can replace J À1 in formula (7), so as to obtain the relationship between the joint angular velocity and the end track velocity of the redundant manipulator as follows: The matrix W 2 R nÂn is a positive diagonal matrix, including the weighting of each joint speed as follows: The following weights are taken: where, v U i and v L i are the maximum speed and the minimum speed of joint i respectively. By weighted pseudo inverse Jacobi matrix, the following formula can be derived: where I is the identity matrix and q 0 0 is the velocity vector. The gradient projection method is used to solve the scalar cost function h q ð Þ of zero space redundancy optimization, and q 0 0 is selected as the derivative of joint cost function, namely: In order to avoid the saturation state of redundant manipulator joints [14,15], the objective function of cost function is: where, a i is the average value of the angular displacement of joint i, y U i and y L i are the upper and lower limit of the angular displacement of joint i, respectively.

Tracking Indexes
For redundant manipulators, the size and direction range of velocity V generated at the end of the i-th joint can be represented by an ellipse in J i (Jacobi matrix space) [16], which is called the traceability ellipse. The main axis of the tracking ellipse is represented by Þof U , the length is represented by the size of d i , and it is generally an ellipse in two-dimensional space and an ellipsoid in three-dimensional space. When the ellipse degenerates into a straight line, it means that the velocity can only be generated in one direction. When the tracking ellipse of the i-th joint is the largest, it has the best tracking performance. The tracking ellipse is shown in Fig. 2.
The size measurement index S Mi (no unit) of the tracking ellipse of the i-th joint is defined as follows: where, m is the dimension of workspace, k m is the coefficient of workspace, and w is the operability index. When m ¼ 2, k m ¼ p; when m is fixed, S Mi is directly proportional to w i .  But sometimes for a given shape, the i-th joint has a high tracking performance, while the other joints have a low tracking performance, which affects the overall tracking performance of redundant manipulator [17]. Therefore, in order to evaluate the overall manipulator, the overall tracking index S M (without unit) is defined as follows: On the other hand, the area of ellipse is used to evaluate the tracking balance between the long axis and the short axis. If the ellipse of the i-th joint is long and thin, the maneuverability of the i-th joint's end along the long axis is high, but the tracking performance along the short axis is low, which will lead to the small S Mi and the small S M , which is not what people expect.

Particle Swarm Optimization Algorithm
PSO algorithm starts from random solution and evaluates the quality of solution by fitness (target value to be optimized), it has the advantages of easy realization, high precision and fast convergence. In the trajectory tracking method of redundant manipulator based on PSO algorithm optimization, the particle swarm optimization algorithm based on contraction factor, it has faster convergence speed than other PSO algorithms. In a n dimensions search space, the population consists of m particles, where the position of the i-th particle is x i 2 R n and its velocity is v i 2 R n ; pbest i 2 R n represents the optimal position of the i-th particle, and gbest 2 R n represents the optimal position of all particles. The particles will change speed and position according to formula (17): where c 1 and c 2 are learning factors, r 1 and r 2 are arbitrary values between [−1,1], and v is shrinkage factor, defined as formula (18): where, ' is the number of redundant manipulator's joints, ' ¼ c 1 þc 2 , ' > 3. According to Fig. 3, the specific implementation process of particle swarm optimization algorithm is as follows:

Contour Location of Redundant Manipulator Based on Active Contour Model
The active contour model can accurately locate the contour of redundant manipulator by converging the curve energy. The active contour model is defined as a continuous elastic curve in two-dimensional image. The contour feature is specified in advance by the energy equation. In the process of curve deformation and motion, the minimum energy is searched to make it gradually approach the feature position from the initial position. If the contour curve of the image is v and the parameter of the contour curve is s, then the energy function with v as the variable is: where, E int ðvðsÞÞ is the internal energy function; E con ðvðsÞÞ is the external energy function. E int ðvðsÞÞ is expressed by formula (20): where, vðsÞ is the derivative of variable v with respect to profile curve parameter s, which requires the profile to be as continuous and smooth as possible; l 1 is the continuity constraint coefficient; l 2 is the smoothness constraint function. By applying external constraint to the curve to make it leave the area where it should not be, different constraint energy functions make the contour converge to different feature positions.
In practical application, the contour curve is expressed as the set K ¼ ðk 0 ; k 1 ; …; k n Þ of multiple controlled points, then the discretization form of the contour energy of redundant mechanical handwheel is as follows: The energy function gets the minimum value at the contour of the manipulator, and the process of finding the minimum energy is the process of searching the characteristic contour of the redundant manipulator, and realizing the positioning of the redundant manipulator contour.

Trajectory Tracking of Redundant Manipulator
According to the contour positioning results of redundant manipulator, the trajectory tracking of redundant manipulator is carried out. As shown in Fig. 4, for the three-joint manipulator in the twodimensional plane, the target point E is known, and the redundant manipulator is limited to the first quadrant to move. At this time, the particle dimension in PSO is taken as 2. The first joint point C is on the circle c with the origin of coordinate system A as the center and the joint length l as the radius, and the second joint point B is on the circle d with the origin of coordinate system E as the center and l as the radius. Therefore, the optimal tracking index S M 3 and the overall optimal tracking index S M at the  Figure 4: Trajectory tracking optimization schematic diagram of three-joint redundant manipulator end of redundant manipulator depend on the selection of B and C. This is also the reason why the algorithm chooses two-dimensional particles, and transforms the calculation of joint angle into the calculation of coordinates of each joint point.
Five particle positions are taken on the circle c and d respectively for initialization and a pair of particles are randomly selected as B and C. The particle velocity is initialized between Àv Ã ; v Ã ½ , and the upper limit of particle velocity is V m . When the particle speed reaches or exceeds V m , let it move in V m . Taking the tracking index as the objective function, the optimal result can be obtained after several generations of evolution. For redundant manipulators (n ≥ 4) with more degrees of freedom in the two-dimensional plane, the process of optimizing the traceability is the same.

Results
In order to verify the trajectory tracking performance of the redundant manipulator based on PSO algorithm proposed in this paper, the three-joint manipulator shown in Fig. 5 is selected as the experimental object, the specific parameters of the manipulator are shown in Tab. 1. The experiment was carried out in the environment of CPU Intel i7-8700, 3.75 GHz; hard disk 1T solid-state drive; video card NVIDIA; memory 32 G; operating system Windows 8.1. In order to avoid the randomness of the data produced in the experiment, the unified initial parameters are set, and the MATLAB software is used to process the data, so as to ensure the reliability of the data.

Tracking Simulation Results
The coordinates of the target point E of the three joint redundant manipulator in the two-dimensional plane are (0.5,0.5). Taking S M 3 as the optimization objective function, 50 experiments are carried out, each time evolving for 10 generations, and an optimal tracking index S M 3 = 6.816 is obtained. At this time, the shape and S M 3 of the redundant manipulator corresponding to the optimal coordinates of each joint point are shown in Fig. 6 and Tab. 2.
It can be seen from Fig. 6 that although the end tracking of the manipulator is the best, the joint angle q 2 is very small. If q 2 becomes zero, then the first and second joints become a straight line, then the redundant manipulator will be in a singular shape and lose a degree of freedom, so it is not enough to only consider the operability of the end of the arm.  When the same experiment is done with S M as the optimization objective function, the optimal overall operability index S M = 10.6883 is obtained, and the shape and S M of the corresponding redundant manipulator are shown in Fig. 7 and Tab. 3.
It can be seen from Fig. 7 that not only the redundant manipulator has a high tracking performance as a whole, but also the size of each joint angle is very balanced, which also shows the superiority of taking S M as the second index.

Trajectory Tracking Comparison Results of Redundant Manipulator
In order to verify the advantages of the proposed method, the proposed method is compared with the control method based on GA-RBF, the control method based on artificial bee colony algorithm and the control method based on PID controller. y U 3 = 0.3 m, y L 1 = 1.2 rad, y L 2 = 0.96 rad, y L 3 = 0 m, and the reference trajectory is q 1 = q 2 = 0.5 cos (π/2) rad, q 3 = 0.2 sin (π/2) m. According to the above parameter  settings, select a joint on the manipulator as the experimental object, and get the simulation results of angular displacement error of different methods of tracking joint motion, as shown in Fig. 8.
According to Fig. 8, with PID control method, the maximum error of joint redundant manipulator is 1.72 × 10 −3 rad, with GA-RBF control method, the maximum error of joint redundant manipulator is 0.91 × 10 −3 rad, with artificial bee colony algorithm, the maximum error of joint redundant manipulator is 1.38 × 10 −3 rad, by using the proposed method, the maximum error of joint redundant manipulator is 0.40 × 10 −4 m. The results show that the method proposed in this paper can track the trajectory of redundant manipulator with high precision.
In order to further verify the effectiveness of the proposed method, take the control rate as the index, compare the proposed method with the control method based on GA-RBF, the control method based on artificial bee colony algorithm and the control method based on PID controller, and the results are shown in Fig. 9.
It can be seen from the analysis of Fig. 9 that when the proposed method is used to track the redundant manipulator, the maximum time required is 2.2 s, while the control time of GA-RBF control method, bee colony control algorithm and PID controller control method is higher than that of the proposed method, which shows that the proposed method has higher control efficiency and is convenient for ordinary operation.
Set the initial position of the manipulator's motion track as the target path, and use the control method based on GA-RBF, the control method based on artificial bee colony algorithm, the control method based on PID controller and the proposed method to track, and get the tracking path of different methods, as shown in Fig. 10.
It can be seen from the analysis of Fig. 10 that the trajectory tracking path obtained by using the proposed method has a higher degree of fit with the target path, which indicates that the proposed method has a better tracking effect and can achieve accurate tracking of redundant manipulator trajectory.

Discussion
With the development of modern science and technology, redundant drive manipulator has been developed rapidly in enterprise production. Because redundant manipulator has many advantages such as stable transmission, sensitive movement and compact structure, it can replace human manual labor and improve productivity. It has been widely used in agriculture, industry, ocean and other fields. Therefore, many countries are researching and developing redundant manipulator. The development of redundant manipulator involves many subjects such as mechanism, control and computer, especially the development of electronic computer technology, which further promotes the research and development of manipulator and improves the design efficiency. In this paper, a trajectory tracking method of redundant manipulator based on PSO algorithm is proposed. The tracking force ellipse evaluates the transfer relationship of static torque force from the joint to the end of the arm. On this basis, this paper studies the shape of redundant manipulator with global tracking index as the second index, and presents a method to optimize the global tracking index by PSO algorithm, that is to say, the point coordinates on the circumference with the relevant joint point as the center and the joint length as the radius are taken as the particles for iterative optimization. In this way, the end of the arm of the redundant manipulator not only completes the given task, but also keeps a high traceability of the whole arm at all times. The simulation results show that the tracking error of each joint of the redundant manipulator is significantly lower than that of the comparison method.
POS algorithm was originally used to simulate the beautiful and unpredictable movement of birds graphically. Through the observation of social behavior of animals, it is found that social sharing of information in groups is conducive to gaining advantages in the evolution, which is the basis for the development of PSO algorithm. PSO algorithm is similar to other evolutionary algorithms, and is also based on the group. It moves the individuals in the group to a good area according to the adaptability to the environment. However, it does not use evolutionary operators for individuals like other evolutionary algorithms, but regards each individual as a particle (point) without volume in the d-dimensional search space, and flies at a certain speed in the search space. This speed is dynamically adjusted according to its own flight experience. PSO algorithm is a very simple algorithm, which only needs a few codes and parameters, but it shows its characteristics and charm in the trajectory tracking of redundant manipulator.

Conclusions
This paper studies the trajectory tracking method of redundant manipulator based on PSO algorithm. On the basis of the tracking index, combined with PSO algorithm, a method of optimizing the tracking index is proposed, that is, the point coordinates on the circle with the relevant joint point as the center and the joint length as the radius are used as the algorithm particles for iterative optimization. The shape of the redundant manipulator can be determined according to the coordinates of each joint point obtained from the optimization results, and the redundant manipulator has the optimal tracking performance at this time. Furthermore, through the experimental simulation, not only the effectiveness of the optimization method proposed in this paper is verified, but also the overall tracking index can be used as the second index to determine the shape of the manipulator. This study not only provides some reference for further theoretical research, but also provides a scheme for practical engineering application.
Funding Statement: This work has been supported by the Ningbo National Natural Science Foundation (2019A610124) and General Project of Education Department of Zhejiang Province (Y201737089).

Conflicts of Interest:
The authors declare that they have no conflicts of interest to report regarding the present study. 414 CMES, 2020, vol.125, no.1