# Taekwondo Trajectory Tracking Based on Multitarget Detection Algorithm

Authors: Liying Zhang; Manchao Li
Journal: Mobile Information Systems
 (2022)

---

## Abstract

In order to further improve the effect of fuzzy PID controller applied to joint robot trajectory tracking control, a Taekwondo trajectory tracking method based on multitarget detection algorithm is proposed in this study. Firstly, a fuzzy PID controller for trajectory tracking of joint robot is designed. Secondly, considering the two optimization objectives of controller output torque and trajectory tracking control deviation, an improved multiobjective PSO algorithm is designed to optimize the membership function and fuzzy rules of fuzzy PID controller. Finally, the multiobjective PSO algorithm and the improved multiobjective PSO algorithm are used to optimize the trajectory tracking fuzzy PID controller. The vector sets of the two optimization objectives are obtained, and the optimization results are compared and analyzed. The simulation results show that the simulation step length ist = 0.01 s, and the total simulation time is 10 s. Using the controller, the simulation values and output torque values of each joint angle at each time can be obtained, which are used to calculate the two objective function values in the optimization algorithm. According to the process, the optimization algorithm parameters are set, and the control simulation system designed above is run to complete the improved multiobjective PSO optimization of the trajectory tracking fuzzy PID controller of the joint robot. Conclusion. The improved multiobjective PSO algorithm has better nondominated solution set, which verifies the effectiveness and superiority of the algorithm to optimize the robot trajectory tracking fuzzy PID controller.


---

## Body

## 1. Introduction

Since the world’s first industrial manipulator was developed in the United States in the 1950s, Japan, Germany, Switzerland, and other countries have successively invested in research in the robot field, forming four robot companies led by Germany KUKA, Japan Yaskawa, FANUC, and Sweden abb. The level of robot technology can reflect a country’s industrial production level to some extent. China has also taken action in the development and promotion of robots. In the “made in China 2025” plan, it is clearly pointed out that the new generation of robot technology should be planned as a national key field, and the application of industrial robots in handling, welding, spraying, assembly and other industrial fields should be promoted [1]. At present, the robots widely used in the market mainly include mobile robots and operating robots. Robots can be divided into series and parallel robots. Serial robot, also known as joint robot and industrial manipulator, is one of the most widely used robots in industry. By installing processing tools such as gripper, sucker, and welding laser at the end of the robot, it can realize the operation tasks on the automatic production line. It has the advantages of multi degrees of freedom, large workspace, and flexibility. The structure of parallel robot is different from that of series robot, as shown in Figure 1.Figure 1
A Taekwondo trajectory tracking method and process based on multiobjective algorithm.Robotics, as a multidisciplinary discipline, its own mechanical structure involves mechanism, control system involves automatic control theory, system state detection involves sensor technology, and system intelligence involves computer programming technology, so it has attracted the attention of academic researchers [2]. Robotics mainly has three research directions: path planning, trajectory planning, and trajectory tracking. Path planning is mainly to determine a feasible path in the robot workspace so that the end effector of the robot can complete the task through the path. Trajectory planning is to obtain the time-varying smooth trajectory of the robot’s end Cartesian position and joint motion parameters through the planning algorithm when the robot’s end path is known and the constraints such as avoiding singular points and acceleration requirements are met. Trajectory tracking refers to the expected trajectory that the robot can complete the trajectory planning through the control algorithm.


## 2. Literature Review

Chen et al. proposed an integral sliding mode trajectory tracking control method for wheeled mobile robots with omni-directional wheels on uneven ground. In this study, the controller is mainly used to solve the external interference caused by uneven ground and omni-directional wheels when driving in a straight line [3]. Wiley Yancy designed a proportional integral derivative (PID) controller for longitudinal trajectory tracking. This method is robust to parameter uncertainty and can reduce output disturbance. This study only focuses on the robust longitudinal trajectory tracking of unmanned trucks with varying mass in uneven agricultural terrain [4]. Farkh et al. used quadric surfaces with unknown but bounded coefficients to approximate the uneven surface and proposed spanning function method, integral backstepping method, and Lyapunov redesign technology to solve the robust stability of wheeled robots moving on uncertain uneven surfaces. In this study, the research on surface approximation and stability control of wheeled robot is completed [5]. Jung and Lee proposed a practical fuzzy lateral control law and proved its global asymptotic stability. Using the virtual prototype technology, the author established the mobile robot and the experimental field on the Adams and MATLAB co-simulation platform and tracked the trajectory of the robot in the simulation environment [6]. In this study, the influence factors of curve radius, road roughness, sensor processing time, and speed requirements are considered, and a human-like driving longitudinal fuzzy control law is designed. The experimental results show that the control method is robust and effective for mobile robots running on rough ground. Lima et al. considered the deformable soil dynamics, deduced the traction force and torque generated by the wheel soil interaction, and completed the path tracking control of the robot based on the terrain. Considering the uncertainty caused by terrain, an adaptive indirect controller is proposed. The controller adopts integrated sliding mode control (ISMC) and adaptive neural networks (NNS: neural networks) to solve the external interference and wheel sliding problems in the robot wheel sliding path tracking control. The design introduces a compensation controller to minimize sudden changes in control requirements, which may be caused by uncertainties related to terrain characteristics and final wheel slip and motion resistance. In this study, the control dynamics based on ISMC is used to formulate adaptive rules, and the Lyapunov stability theorem is used to ensure the stability of the closed-loop system. At the same time, the effectiveness of the controller is verified on sandy soil with typical linear and curvilinear trajectory. In the design of the trajectory tracking controller, the deformable soil dynamics is considered to obtain the traction force and torque generated by the wheel terrain interaction, the uncertainty caused by the terrain, and the dynamics of the sliding surface. The design process of the controller does not involve the control of torque limitation [7]. Chin et al. proposed a model-based control scheme and an adaptive neural network control law. They solved the constraint control of input speed, but did not study the torque constraint and the trajectory tracking of surfaces. Although the torque can be indirectly limited by limiting the speed, the torque is an important input of the robot system. Direct control of the input torque can obtain better control effect on the torque [8].In this study, taking the controller output torque and trajectory tracking deviation as two optimization objectives, the improved multiobjective PSO algorithm is implemented to optimize the membership function of the fuzzy controller and the optimal adjustment of fuzzy rules. Firstly, a fuzzy PID controller for trajectory tracking of Taekwondo robot is designed. Then, according to the multiobjective optimization problem of the fuzzy PID controller, an improved multiobjective PSO algorithm is designed; Finally, the improved multiobjective PSO and the basic multiobjective PSO are applied to the multiobjective optimization of fuzzy PID control, and the optimization results of the two optimization algorithms are compared and analyzed.


## 3. Research Methods

### 3.1. Robot Trajectory Tracking Fuzzy PID Control

The track tracking fuzzy PID control system of Taekwondo robot is shown in Figure2. In the figure, qd and q represent the expected and actual angles of each joint of the six-joint robot, respectively; e represents the deviation between qd and q, e=qd−q; ec represents deviation change rate, ec=de/dt. The dotted line in Figure 2 represents the fuzzy controller, where e and ec are the input variables of the fuzzy controller, ΔKp, ΔK1, and ΔKD represent the PID adjustment parameters, and τ is the output variable of the fuzzy controller, representing the output control torque of the fuzzy PID controller [9, 10]. The control process is as follows. Firstly, the fuzzy controller obtains PID adjustment parameters through fuzzification, fuzzy reasoning, and defuzzification according to the value of sum. Then, we realize the online self-adaptive adjustment of three parameters of PID controller, such as equation (1), and output the control torque according to equation (2). Finally, the control torque is applied to the robot to obtain the actual joint angle, which is fed back to the comparator:(1)Kp=KP0+ΔKp,K1=K10+ΔK1,KD=KD0+ΔKD,(2)τt=KPe+K1∫02edt+KDdedt,where KP0, K10, and KD0 are the initial values of PID parameters and T is the time.Figure 2
Robot trajectory tracking fuzzy PID control system.


#### 3.1.1. Design of Fuzzy PID Controller

In the fuzzy PID controller, the number of fuzzy sets of input variable deviatione, deviation change rate ec, and output variables ΔKp, ΔK1, and ΔKD are designed to be 7, and the fuzzy sets are taken as {NL, NM, NS, ZE, PS, PM, PL}, that is, {negative large, negative medium, negative small, zero, positive small, positive medium, positive large} [11]. The fuzzy set membership function selects the triangular membership function. According to the characteristics of the membership function, the membership functions of all fuzzy sets of a variable are symmetrical about the Y-axis, and the “left foot” and “right foot” of a triangular membership function are, respectively, set as the abscissa of the vertices of the adjacent left and right triangular membership functions. Therefore, when designing the membership function of each variable, it is necessary to determine its universe and three parameters X1, X2, and X3 on one side of the Y-axis [12]. The specific parameters of the membership functions of all variables of the fuzzy PID controller are shown in Table 1.Table 1
Membership function design of input and output variables of the fuzzy PID controller.
Variable namesDomain of discourseX1X2X3e10.330.661ec10.330.661ΔKp103.36.610ΔK1103.36.610ΔKD103.36.610The number of fuzzy sets of two input variablese and ec is 7. It can be obtained that each variable of output variables ΔKp, ΔK1, and ΔKD corresponds to 49 fuzzy rules. The form of each fuzzy rule isife is⋯and ec is⋯,thenΔKp is ⋯ΔK1 is ⋯, ΔKD is ⋯


#### 3.1.2. Robot Dynamic Model

Ignoring joint friction and end load, the dynamic equation of the six joint robot can be written as the following formula:(3)τ=Mqq¨+Cq,q˙q˙+Gq,where q, q˙, and q¨∈R6 are angle vector, angular velocity vector, and angular acceleration vector composed of robot joints, angular velocity, and angular acceleration, respectively; Mq∈R6×6 is inertia symmetry matrix; Cq,q˙∈R6×6− is the Coriolis force and centrifugal force coefficient matrix; Gq∈R6 is the gravity vector [13].In order to simulate the six joint robot, the joint angular accelerationq¨t in the equation can be calculated according to the dynamic equation (3) established above:(4)q¨t=M−1qtτt−Cqt,q˙tq˙t−Gqt,where qt and q˙t are the known robot joint angle and joint angular velocity at a certain t time, q¨t is the joint angular acceleration at a time t, τt is the control torque output by the controller at a time t, and Mqt and Cqt,q˙tq˙t+Gqt can be obtained from qt,q˙t.From the joint angular accelerationq¨t at time t, assuming that q¨t does not change within the time step Δt and under the initial condition of q0 = q˙0=0, the joint information qt and q˙t at the current time t are numerically integrated, and the angular velocity q˙t+Δt and joint angle qt+Δt at the next time t+Δt are calculated as follows:(5)q˙t+Δs=q˙t+q¨tΔt,qt+Δt=qt+q˙tΔt+12q¨1Δt2.


### 3.2. Multiobjective PSO Optimization of Fuzzy PID Control

#### 3.2.1. Multiobjective Problem of Fuzzy PID Control

To optimize the robot trajectory tracking fuzzy PID control, it is necessary to encode the parameters to be optimized to form a decision vector [14]. The decision vector in the particle swarm optimization algorithm is the position vector xi of a particle i, which is specifically expressed as the following formula:(6)xi=x1,x2,⋯,xD,where D is the number of parameters to be optimized in the optimization problem.The parameters represented by elementxi in the particle position vector of PSO optimized fuzzy PID control and the range of parameter values are shown in Table 2. Element xt,1−xt,15 represents the central vertex value of the membership function of input variables e and ec and output variables ΔKp, ΔK1, and ΔKD, respectively. Each variable needs to specify 3 central vertex values, so 15 design parameters representing the membership function of fuzzy PID controller are required for 5 variables; The fuzzy PID controller has three rule tables in total, and each rule table has 49 fuzzy rule results to be designed, so 3 × 49 = 147 element representation is required; that is, xi,16−xi,64,xi,65−xi,113,xi,114−xi,162 represent 49 fuzzy rules of ΔKp,ΔK1,ΔKD rules, respectively. Therefore, a total of 162 element bases are required, D = 162.Table 2
NSGAII optimization algorithm parameters.
AlgorithmNSGAIIPopulation size100Iterations50Selection methodRoulette methodCopy scale0.2Crossover modeTwo-point crossoverCrossover probability0.8Probability of variation0.05The fuzzy PID control has two optimization objective functions, which are, respectively, related to the trajectory tracking deviation and the controller output torque. As shown in equation (7), the objective vector is f = f1,f2:(7)f1xi=∫0Tt⋅etdt,f2xi=∫0Tt∙∑i=16τntdt,where et is the deviation between the robot Cartesian space simulation trajectory and the expected trajectory at time t, τnt is the output torque of the fuzzy PID controller acting on the nth joint at time t, and T is the total time for running the fuzzy PID controller to complete a trajectory tracking control simulation [15, 16].In the multiobjective optimization of fuzzy PID controller, minimizing the controller output torque may lead to the increase of trajectory tracking error. Therefore, it is necessary to find a balanced solution between the two conflicting objectives, and such a solution is not unique. If the position vectorsx1 and x2 of two particles have fkx1≤fkx2 ≤ for ∀k=1, 2 and fkx1＜fkx2 for ∃k=1, x1 dominates x2. Otherwise, it is called non dominant relationship. The task of multiobjective optimization algorithm is to find the set of objective vectors f = f1,f2 corresponding to all particles that are not dominated by other particle position vectors. These objective vectors are nondominated relations with each other, and their set is called nondominated solution set.


#### 3.2.2. Improved Multiobjective PSO Algorithm

Particle swarm optimization algorithm is to continuously adjust the position of each particle to make the particle move to the optimal position [17]. Each particle in the algorithm has a position vector xik shaped like xi,1k,xi,2k,…,xi,Dk and a velocity vector vik shaped like xi,1k,xi,2k,…,xi,Dk, where i = 1, 2, …, S is the sequence number of particles in the particle swarm, k is the number of iterations, and S is the number of particle swarm.The main steps of the algorithm are as follows.(1)
Generate initial particle population: randomly generateS particle position vectors xi0 with the shape of xi,10,xi,20,…,xi,1620, and the initial population is {x10,x20,…,xs0}. Set the velocity vector vi0 = 0 for all particles in the initial population.(2)
Update Particle Swarm. Set thekth generation particle swarm as the current particle swarm.(3)
Update membership function and fuzzy rules. Read the position vector of thejj=1,2,…,Sth particle in the current particle swarm, generate new membership functions of input variables e, ec and output variables ΔKp,ΔK1,ΔKD according to code xi,1k−xi,10k, and update the membership function; The fuzzy rules of new output variables ΔKp、ΔK1、ΔKD are generated according to code xi,11k−xi,63k, and the fuzzy rules are updated.(4)
Calculate the objective function value. Run the fuzzy PID controller to complete the trajectory tracking control simulation, obtain the deviationet between the robot simulation trajectory and the expected trajectory at time t in the simulation control and the output uit of each joint controller, and calculate the objective vector f = f1,f2 according to equation (7).(5)
Judge whether to traverseS particles. If the number of particles j does not meet j＞S, then j+1, go to step 3). If j＞S, go to step 6).(6)
Update individual file and global file. Individual file and global file are nondominated solution sets that store individual optimal positions and global optimal particles, respectively. Each particle has its own individual file, while the global file is shared by all particles. The individual file and the global file are prepared for selectingpbest and gbest in step 8. If the particle in the contemporary particle swarm has a nondominant relationship with the solution set in the file, the particle will be added to the file [18].(7)
Judge whether to terminate the optimization algorithm. Judge whether the number of particle iterations reaches the set value. If the maximum value is reached, the optimization algorithm will be ended and the particle position vector in the current global file will be returned. If not, proceed to step 8).(8)
Selectpbest and gbest. Select a certain value from the nondominated solution set in the individual file and the global file [17]. The specific method for selecting pbest and gbest is as follows. First, each particle is assigned a value σi, and each particle in the personal file and global file is also assigned a value σpi and σgi. The particle value with the objective function value is defined as follows:(8)σ=f12−f22f12+f22.Then, the file members ofσi whose particles σpi and σgi are closest to particle i in the individual file and the global file are used as the guides pbest and gbest of the particle.(9)
Parameters required for calculating the velocity vector: the parameters required for the velocity vector are the inertia weightωk and positive acceleration coefficient c1k and c2k of the kth generation particle, respectively. The specific calculation formula is as follows:(9)ωk=ω0+ωnk−ω0knk,wherenk is the maximum number of iterations set by the algorithm, ω0 and ωnk are initial inertia weight and final inertia weight, respectively, and ωk is the inertia weight of generation k, and ω0≥ωnk:(10)c1k=c10+c1nk−c10knk,c2k=c20+c2nk−c20knk,wherec10 and c1nk are the initial and final values of c1, respectively, c20 and c2nk are the initial and final values of c2 respectively, and c10≥c1nk and c20≤c2nk.(10)
Calculate the particle velocity vector. The second dimension velocity in the velocity vectorvik+1 of the next iteration is calculated as follows:(11)vi,dk+1=vi,d′k+1ifvi,d′k+1<Vmax,d,Vmax,difvi,d′k+1⩾Vmax,d,whereVmax,d is the maximum allowable speed in the d dimension; vi,d′k+1 calculation is shown as follows:(12)vi,d′k+1=ωvi,dk+c1r1pbesti,dk−xi,dk+c2r2gbestdk−xi,dk,wherexi,dk and vi,dk are, respectively, the position and velocity of k in the d dimension of particle i at time, d=1,2,…,D, ω is inertia weight coefficient, pbesti,dk is the dth dimension of pbest selected by particle I from the individual file, and gbestdk is the dth dimension of gbest selected from the current global file at the time; The coefficients r1 and r2 are random numbers subject to uniform distribution U0,1 in the interval [0, 1].(11)
Update particle position: update particle position vectorxik+1 is as follows:(13)xik+1=xik+vik+1.(12)
Particle position mutation: mutate the particle position in the new particle swarm according to equation (14), and go to step (2)::(14)xi,d=xd,max−xd,min⋅r+xd,min,where xi,d is the value of particle i in the d dimension and xd,max and xd,min are the upper and lower bounds of the particle in the upper dimension, respectively. The coefficient r is a random number [19, 20] that obeys the uniform distribution U0,1 on the interval [0, 1].


## 3.1. Robot Trajectory Tracking Fuzzy PID Control

The track tracking fuzzy PID control system of Taekwondo robot is shown in Figure2. In the figure, qd and q represent the expected and actual angles of each joint of the six-joint robot, respectively; e represents the deviation between qd and q, e=qd−q; ec represents deviation change rate, ec=de/dt. The dotted line in Figure 2 represents the fuzzy controller, where e and ec are the input variables of the fuzzy controller, ΔKp, ΔK1, and ΔKD represent the PID adjustment parameters, and τ is the output variable of the fuzzy controller, representing the output control torque of the fuzzy PID controller [9, 10]. The control process is as follows. Firstly, the fuzzy controller obtains PID adjustment parameters through fuzzification, fuzzy reasoning, and defuzzification according to the value of sum. Then, we realize the online self-adaptive adjustment of three parameters of PID controller, such as equation (1), and output the control torque according to equation (2). Finally, the control torque is applied to the robot to obtain the actual joint angle, which is fed back to the comparator:(1)Kp=KP0+ΔKp,K1=K10+ΔK1,KD=KD0+ΔKD,(2)τt=KPe+K1∫02edt+KDdedt,where KP0, K10, and KD0 are the initial values of PID parameters and T is the time.Figure 2
Robot trajectory tracking fuzzy PID control system.


### 3.1.1. Design of Fuzzy PID Controller

In the fuzzy PID controller, the number of fuzzy sets of input variable deviatione, deviation change rate ec, and output variables ΔKp, ΔK1, and ΔKD are designed to be 7, and the fuzzy sets are taken as {NL, NM, NS, ZE, PS, PM, PL}, that is, {negative large, negative medium, negative small, zero, positive small, positive medium, positive large} [11]. The fuzzy set membership function selects the triangular membership function. According to the characteristics of the membership function, the membership functions of all fuzzy sets of a variable are symmetrical about the Y-axis, and the “left foot” and “right foot” of a triangular membership function are, respectively, set as the abscissa of the vertices of the adjacent left and right triangular membership functions. Therefore, when designing the membership function of each variable, it is necessary to determine its universe and three parameters X1, X2, and X3 on one side of the Y-axis [12]. The specific parameters of the membership functions of all variables of the fuzzy PID controller are shown in Table 1.Table 1
Membership function design of input and output variables of the fuzzy PID controller.
Variable namesDomain of discourseX1X2X3e10.330.661ec10.330.661ΔKp103.36.610ΔK1103.36.610ΔKD103.36.610The number of fuzzy sets of two input variablese and ec is 7. It can be obtained that each variable of output variables ΔKp, ΔK1, and ΔKD corresponds to 49 fuzzy rules. The form of each fuzzy rule isife is⋯and ec is⋯,thenΔKp is ⋯ΔK1 is ⋯, ΔKD is ⋯


### 3.1.2. Robot Dynamic Model

Ignoring joint friction and end load, the dynamic equation of the six joint robot can be written as the following formula:(3)τ=Mqq¨+Cq,q˙q˙+Gq,where q, q˙, and q¨∈R6 are angle vector, angular velocity vector, and angular acceleration vector composed of robot joints, angular velocity, and angular acceleration, respectively; Mq∈R6×6 is inertia symmetry matrix; Cq,q˙∈R6×6− is the Coriolis force and centrifugal force coefficient matrix; Gq∈R6 is the gravity vector [13].In order to simulate the six joint robot, the joint angular accelerationq¨t in the equation can be calculated according to the dynamic equation (3) established above:(4)q¨t=M−1qtτt−Cqt,q˙tq˙t−Gqt,where qt and q˙t are the known robot joint angle and joint angular velocity at a certain t time, q¨t is the joint angular acceleration at a time t, τt is the control torque output by the controller at a time t, and Mqt and Cqt,q˙tq˙t+Gqt can be obtained from qt,q˙t.From the joint angular accelerationq¨t at time t, assuming that q¨t does not change within the time step Δt and under the initial condition of q0 = q˙0=0, the joint information qt and q˙t at the current time t are numerically integrated, and the angular velocity q˙t+Δt and joint angle qt+Δt at the next time t+Δt are calculated as follows:(5)q˙t+Δs=q˙t+q¨tΔt,qt+Δt=qt+q˙tΔt+12q¨1Δt2.


## 3.1.1. Design of Fuzzy PID Controller

In the fuzzy PID controller, the number of fuzzy sets of input variable deviatione, deviation change rate ec, and output variables ΔKp, ΔK1, and ΔKD are designed to be 7, and the fuzzy sets are taken as {NL, NM, NS, ZE, PS, PM, PL}, that is, {negative large, negative medium, negative small, zero, positive small, positive medium, positive large} [11]. The fuzzy set membership function selects the triangular membership function. According to the characteristics of the membership function, the membership functions of all fuzzy sets of a variable are symmetrical about the Y-axis, and the “left foot” and “right foot” of a triangular membership function are, respectively, set as the abscissa of the vertices of the adjacent left and right triangular membership functions. Therefore, when designing the membership function of each variable, it is necessary to determine its universe and three parameters X1, X2, and X3 on one side of the Y-axis [12]. The specific parameters of the membership functions of all variables of the fuzzy PID controller are shown in Table 1.Table 1
Membership function design of input and output variables of the fuzzy PID controller.
Variable namesDomain of discourseX1X2X3e10.330.661ec10.330.661ΔKp103.36.610ΔK1103.36.610ΔKD103.36.610The number of fuzzy sets of two input variablese and ec is 7. It can be obtained that each variable of output variables ΔKp, ΔK1, and ΔKD corresponds to 49 fuzzy rules. The form of each fuzzy rule isife is⋯and ec is⋯,thenΔKp is ⋯ΔK1 is ⋯, ΔKD is ⋯


## 3.1.2. Robot Dynamic Model

Ignoring joint friction and end load, the dynamic equation of the six joint robot can be written as the following formula:(3)τ=Mqq¨+Cq,q˙q˙+Gq,where q, q˙, and q¨∈R6 are angle vector, angular velocity vector, and angular acceleration vector composed of robot joints, angular velocity, and angular acceleration, respectively; Mq∈R6×6 is inertia symmetry matrix; Cq,q˙∈R6×6− is the Coriolis force and centrifugal force coefficient matrix; Gq∈R6 is the gravity vector [13].In order to simulate the six joint robot, the joint angular accelerationq¨t in the equation can be calculated according to the dynamic equation (3) established above:(4)q¨t=M−1qtτt−Cqt,q˙tq˙t−Gqt,where qt and q˙t are the known robot joint angle and joint angular velocity at a certain t time, q¨t is the joint angular acceleration at a time t, τt is the control torque output by the controller at a time t, and Mqt and Cqt,q˙tq˙t+Gqt can be obtained from qt,q˙t.From the joint angular accelerationq¨t at time t, assuming that q¨t does not change within the time step Δt and under the initial condition of q0 = q˙0=0, the joint information qt and q˙t at the current time t are numerically integrated, and the angular velocity q˙t+Δt and joint angle qt+Δt at the next time t+Δt are calculated as follows:(5)q˙t+Δs=q˙t+q¨tΔt,qt+Δt=qt+q˙tΔt+12q¨1Δt2.


## 3.2. Multiobjective PSO Optimization of Fuzzy PID Control

### 3.2.1. Multiobjective Problem of Fuzzy PID Control

To optimize the robot trajectory tracking fuzzy PID control, it is necessary to encode the parameters to be optimized to form a decision vector [14]. The decision vector in the particle swarm optimization algorithm is the position vector xi of a particle i, which is specifically expressed as the following formula:(6)xi=x1,x2,⋯,xD,where D is the number of parameters to be optimized in the optimization problem.The parameters represented by elementxi in the particle position vector of PSO optimized fuzzy PID control and the range of parameter values are shown in Table 2. Element xt,1−xt,15 represents the central vertex value of the membership function of input variables e and ec and output variables ΔKp, ΔK1, and ΔKD, respectively. Each variable needs to specify 3 central vertex values, so 15 design parameters representing the membership function of fuzzy PID controller are required for 5 variables; The fuzzy PID controller has three rule tables in total, and each rule table has 49 fuzzy rule results to be designed, so 3 × 49 = 147 element representation is required; that is, xi,16−xi,64,xi,65−xi,113,xi,114−xi,162 represent 49 fuzzy rules of ΔKp,ΔK1,ΔKD rules, respectively. Therefore, a total of 162 element bases are required, D = 162.Table 2
NSGAII optimization algorithm parameters.
AlgorithmNSGAIIPopulation size100Iterations50Selection methodRoulette methodCopy scale0.2Crossover modeTwo-point crossoverCrossover probability0.8Probability of variation0.05The fuzzy PID control has two optimization objective functions, which are, respectively, related to the trajectory tracking deviation and the controller output torque. As shown in equation (7), the objective vector is f = f1,f2:(7)f1xi=∫0Tt⋅etdt,f2xi=∫0Tt∙∑i=16τntdt,where et is the deviation between the robot Cartesian space simulation trajectory and the expected trajectory at time t, τnt is the output torque of the fuzzy PID controller acting on the nth joint at time t, and T is the total time for running the fuzzy PID controller to complete a trajectory tracking control simulation [15, 16].In the multiobjective optimization of fuzzy PID controller, minimizing the controller output torque may lead to the increase of trajectory tracking error. Therefore, it is necessary to find a balanced solution between the two conflicting objectives, and such a solution is not unique. If the position vectorsx1 and x2 of two particles have fkx1≤fkx2 ≤ for ∀k=1, 2 and fkx1＜fkx2 for ∃k=1, x1 dominates x2. Otherwise, it is called non dominant relationship. The task of multiobjective optimization algorithm is to find the set of objective vectors f = f1,f2 corresponding to all particles that are not dominated by other particle position vectors. These objective vectors are nondominated relations with each other, and their set is called nondominated solution set.


### 3.2.2. Improved Multiobjective PSO Algorithm

Particle swarm optimization algorithm is to continuously adjust the position of each particle to make the particle move to the optimal position [17]. Each particle in the algorithm has a position vector xik shaped like xi,1k,xi,2k,…,xi,Dk and a velocity vector vik shaped like xi,1k,xi,2k,…,xi,Dk, where i = 1, 2, …, S is the sequence number of particles in the particle swarm, k is the number of iterations, and S is the number of particle swarm.The main steps of the algorithm are as follows.(1)
Generate initial particle population: randomly generateS particle position vectors xi0 with the shape of xi,10,xi,20,…,xi,1620, and the initial population is {x10,x20,…,xs0}. Set the velocity vector vi0 = 0 for all particles in the initial population.(2)
Update Particle Swarm. Set thekth generation particle swarm as the current particle swarm.(3)
Update membership function and fuzzy rules. Read the position vector of thejj=1,2,…,Sth particle in the current particle swarm, generate new membership functions of input variables e, ec and output variables ΔKp,ΔK1,ΔKD according to code xi,1k−xi,10k, and update the membership function; The fuzzy rules of new output variables ΔKp、ΔK1、ΔKD are generated according to code xi,11k−xi,63k, and the fuzzy rules are updated.(4)
Calculate the objective function value. Run the fuzzy PID controller to complete the trajectory tracking control simulation, obtain the deviationet between the robot simulation trajectory and the expected trajectory at time t in the simulation control and the output uit of each joint controller, and calculate the objective vector f = f1,f2 according to equation (7).(5)
Judge whether to traverseS particles. If the number of particles j does not meet j＞S, then j+1, go to step 3). If j＞S, go to step 6).(6)
Update individual file and global file. Individual file and global file are nondominated solution sets that store individual optimal positions and global optimal particles, respectively. Each particle has its own individual file, while the global file is shared by all particles. The individual file and the global file are prepared for selectingpbest and gbest in step 8. If the particle in the contemporary particle swarm has a nondominant relationship with the solution set in the file, the particle will be added to the file [18].(7)
Judge whether to terminate the optimization algorithm. Judge whether the number of particle iterations reaches the set value. If the maximum value is reached, the optimization algorithm will be ended and the particle position vector in the current global file will be returned. If not, proceed to step 8).(8)
Selectpbest and gbest. Select a certain value from the nondominated solution set in the individual file and the global file [17]. The specific method for selecting pbest and gbest is as follows. First, each particle is assigned a value σi, and each particle in the personal file and global file is also assigned a value σpi and σgi. The particle value with the objective function value is defined as follows:(8)σ=f12−f22f12+f22.Then, the file members ofσi whose particles σpi and σgi are closest to particle i in the individual file and the global file are used as the guides pbest and gbest of the particle.(9)
Parameters required for calculating the velocity vector: the parameters required for the velocity vector are the inertia weightωk and positive acceleration coefficient c1k and c2k of the kth generation particle, respectively. The specific calculation formula is as follows:(9)ωk=ω0+ωnk−ω0knk,wherenk is the maximum number of iterations set by the algorithm, ω0 and ωnk are initial inertia weight and final inertia weight, respectively, and ωk is the inertia weight of generation k, and ω0≥ωnk:(10)c1k=c10+c1nk−c10knk,c2k=c20+c2nk−c20knk,wherec10 and c1nk are the initial and final values of c1, respectively, c20 and c2nk are the initial and final values of c2 respectively, and c10≥c1nk and c20≤c2nk.(10)
Calculate the particle velocity vector. The second dimension velocity in the velocity vectorvik+1 of the next iteration is calculated as follows:(11)vi,dk+1=vi,d′k+1ifvi,d′k+1<Vmax,d,Vmax,difvi,d′k+1⩾Vmax,d,whereVmax,d is the maximum allowable speed in the d dimension; vi,d′k+1 calculation is shown as follows:(12)vi,d′k+1=ωvi,dk+c1r1pbesti,dk−xi,dk+c2r2gbestdk−xi,dk,wherexi,dk and vi,dk are, respectively, the position and velocity of k in the d dimension of particle i at time, d=1,2,…,D, ω is inertia weight coefficient, pbesti,dk is the dth dimension of pbest selected by particle I from the individual file, and gbestdk is the dth dimension of gbest selected from the current global file at the time; The coefficients r1 and r2 are random numbers subject to uniform distribution U0,1 in the interval [0, 1].(11)
Update particle position: update particle position vectorxik+1 is as follows:(13)xik+1=xik+vik+1.(12)
Particle position mutation: mutate the particle position in the new particle swarm according to equation (14), and go to step (2)::(14)xi,d=xd,max−xd,min⋅r+xd,min,where xi,d is the value of particle i in the d dimension and xd,max and xd,min are the upper and lower bounds of the particle in the upper dimension, respectively. The coefficient r is a random number [19, 20] that obeys the uniform distribution U0,1 on the interval [0, 1].


## 3.2.1. Multiobjective Problem of Fuzzy PID Control

To optimize the robot trajectory tracking fuzzy PID control, it is necessary to encode the parameters to be optimized to form a decision vector [14]. The decision vector in the particle swarm optimization algorithm is the position vector xi of a particle i, which is specifically expressed as the following formula:(6)xi=x1,x2,⋯,xD,where D is the number of parameters to be optimized in the optimization problem.The parameters represented by elementxi in the particle position vector of PSO optimized fuzzy PID control and the range of parameter values are shown in Table 2. Element xt,1−xt,15 represents the central vertex value of the membership function of input variables e and ec and output variables ΔKp, ΔK1, and ΔKD, respectively. Each variable needs to specify 3 central vertex values, so 15 design parameters representing the membership function of fuzzy PID controller are required for 5 variables; The fuzzy PID controller has three rule tables in total, and each rule table has 49 fuzzy rule results to be designed, so 3 × 49 = 147 element representation is required; that is, xi,16−xi,64,xi,65−xi,113,xi,114−xi,162 represent 49 fuzzy rules of ΔKp,ΔK1,ΔKD rules, respectively. Therefore, a total of 162 element bases are required, D = 162.Table 2
NSGAII optimization algorithm parameters.
AlgorithmNSGAIIPopulation size100Iterations50Selection methodRoulette methodCopy scale0.2Crossover modeTwo-point crossoverCrossover probability0.8Probability of variation0.05The fuzzy PID control has two optimization objective functions, which are, respectively, related to the trajectory tracking deviation and the controller output torque. As shown in equation (7), the objective vector is f = f1,f2:(7)f1xi=∫0Tt⋅etdt,f2xi=∫0Tt∙∑i=16τntdt,where et is the deviation between the robot Cartesian space simulation trajectory and the expected trajectory at time t, τnt is the output torque of the fuzzy PID controller acting on the nth joint at time t, and T is the total time for running the fuzzy PID controller to complete a trajectory tracking control simulation [15, 16].In the multiobjective optimization of fuzzy PID controller, minimizing the controller output torque may lead to the increase of trajectory tracking error. Therefore, it is necessary to find a balanced solution between the two conflicting objectives, and such a solution is not unique. If the position vectorsx1 and x2 of two particles have fkx1≤fkx2 ≤ for ∀k=1, 2 and fkx1＜fkx2 for ∃k=1, x1 dominates x2. Otherwise, it is called non dominant relationship. The task of multiobjective optimization algorithm is to find the set of objective vectors f = f1,f2 corresponding to all particles that are not dominated by other particle position vectors. These objective vectors are nondominated relations with each other, and their set is called nondominated solution set.


## 3.2.2. Improved Multiobjective PSO Algorithm

Particle swarm optimization algorithm is to continuously adjust the position of each particle to make the particle move to the optimal position [17]. Each particle in the algorithm has a position vector xik shaped like xi,1k,xi,2k,…,xi,Dk and a velocity vector vik shaped like xi,1k,xi,2k,…,xi,Dk, where i = 1, 2, …, S is the sequence number of particles in the particle swarm, k is the number of iterations, and S is the number of particle swarm.The main steps of the algorithm are as follows.(1)
Generate initial particle population: randomly generateS particle position vectors xi0 with the shape of xi,10,xi,20,…,xi,1620, and the initial population is {x10,x20,…,xs0}. Set the velocity vector vi0 = 0 for all particles in the initial population.(2)
Update Particle Swarm. Set thekth generation particle swarm as the current particle swarm.(3)
Update membership function and fuzzy rules. Read the position vector of thejj=1,2,…,Sth particle in the current particle swarm, generate new membership functions of input variables e, ec and output variables ΔKp,ΔK1,ΔKD according to code xi,1k−xi,10k, and update the membership function; The fuzzy rules of new output variables ΔKp、ΔK1、ΔKD are generated according to code xi,11k−xi,63k, and the fuzzy rules are updated.(4)
Calculate the objective function value. Run the fuzzy PID controller to complete the trajectory tracking control simulation, obtain the deviationet between the robot simulation trajectory and the expected trajectory at time t in the simulation control and the output uit of each joint controller, and calculate the objective vector f = f1,f2 according to equation (7).(5)
Judge whether to traverseS particles. If the number of particles j does not meet j＞S, then j+1, go to step 3). If j＞S, go to step 6).(6)
Update individual file and global file. Individual file and global file are nondominated solution sets that store individual optimal positions and global optimal particles, respectively. Each particle has its own individual file, while the global file is shared by all particles. The individual file and the global file are prepared for selectingpbest and gbest in step 8. If the particle in the contemporary particle swarm has a nondominant relationship with the solution set in the file, the particle will be added to the file [18].(7)
Judge whether to terminate the optimization algorithm. Judge whether the number of particle iterations reaches the set value. If the maximum value is reached, the optimization algorithm will be ended and the particle position vector in the current global file will be returned. If not, proceed to step 8).(8)
Selectpbest and gbest. Select a certain value from the nondominated solution set in the individual file and the global file [17]. The specific method for selecting pbest and gbest is as follows. First, each particle is assigned a value σi, and each particle in the personal file and global file is also assigned a value σpi and σgi. The particle value with the objective function value is defined as follows:(8)σ=f12−f22f12+f22.Then, the file members ofσi whose particles σpi and σgi are closest to particle i in the individual file and the global file are used as the guides pbest and gbest of the particle.(9)
Parameters required for calculating the velocity vector: the parameters required for the velocity vector are the inertia weightωk and positive acceleration coefficient c1k and c2k of the kth generation particle, respectively. The specific calculation formula is as follows:(9)ωk=ω0+ωnk−ω0knk,wherenk is the maximum number of iterations set by the algorithm, ω0 and ωnk are initial inertia weight and final inertia weight, respectively, and ωk is the inertia weight of generation k, and ω0≥ωnk:(10)c1k=c10+c1nk−c10knk,c2k=c20+c2nk−c20knk,wherec10 and c1nk are the initial and final values of c1, respectively, c20 and c2nk are the initial and final values of c2 respectively, and c10≥c1nk and c20≤c2nk.(10)
Calculate the particle velocity vector. The second dimension velocity in the velocity vectorvik+1 of the next iteration is calculated as follows:(11)vi,dk+1=vi,d′k+1ifvi,d′k+1<Vmax,d,Vmax,difvi,d′k+1⩾Vmax,d,whereVmax,d is the maximum allowable speed in the d dimension; vi,d′k+1 calculation is shown as follows:(12)vi,d′k+1=ωvi,dk+c1r1pbesti,dk−xi,dk+c2r2gbestdk−xi,dk,wherexi,dk and vi,dk are, respectively, the position and velocity of k in the d dimension of particle i at time, d=1,2,…,D, ω is inertia weight coefficient, pbesti,dk is the dth dimension of pbest selected by particle I from the individual file, and gbestdk is the dth dimension of gbest selected from the current global file at the time; The coefficients r1 and r2 are random numbers subject to uniform distribution U0,1 in the interval [0, 1].(11)
Update particle position: update particle position vectorxik+1 is as follows:(13)xik+1=xik+vik+1.(12)
Particle position mutation: mutate the particle position in the new particle swarm according to equation (14), and go to step (2)::(14)xi,d=xd,max−xd,min⋅r+xd,min,where xi,d is the value of particle i in the d dimension and xd,max and xd,min are the upper and lower bounds of the particle in the upper dimension, respectively. The coefficient r is a random number [19, 20] that obeys the uniform distribution U0,1 on the interval [0, 1].


## 4. Result Analysis

In the robot trajectory tracking control simulation, the desired trajectory in Cartesian space is assumed to be a spiral, and its parameter equation is as follows:(15)x=0.45+0.25⋅cost⋅2π/Tc,y=0.25+0.25⋅sint⋅2π/Tc.z=−0.25+0.025⋅t,where t is time, Tc is the cycle duration, and x, y, and z are coordinate values of Cartesian space, respectively.According to robot kinematics, the desired trajectory in Cartesian space can be converted into the desired angle trajectory of each joint in robot joint space, as shown in Figure3 [21]. The MATLAB simulation system of robot trajectory tracking fuzzy PID control built in this study has a simulation step of Δt = 0.01 s and a total simulation time of T = 10 s. Using the controller, the simulation values and output torque values of each joint angle at each time can be obtained, which are used to calculate the two objective function values f1 and f2 of equation (7) in the optimization algorithm. According to the process, the optimization algorithm parameters are set, and the control simulation system designed above is run to complete the improved multiobjective PSO optimization of the joint robot trajectory tracking fuzzy PID controller.Figure 3
Expected angle trajectory of joint space.Nondominated sorting genetic algorithm with elite strategy, multiobjective basic particle swarm optimization (MOPSO), and modified multiobjective particle swarm optimization (ModifyMOPSO) are used to optimize the trajectory tracking fuzzy PID controller. The parameter settings of the optimization algorithm are shown in Table2. The nondominated solution set optimized by the three optimization algorithms is shown in Figure 4, and the two objective function values of each solution in the solution set are shown in Table 3. As can be seen from Figure 4, the solution set of ModifyPSO is widely and evenly distributed, while some solution sets obtained by MOPSO are concentrated in a certain area and cover a small range [22, 23]. Although the solution set obtained by NSGA II is widely distributed, it is unevenly distributed. A hypervolume index that can simultaneously evaluate the convergence and diversity of solution sets is used to compare the nondominated solution sets obtained by the three optimization algorithms. The larger the hypervolume, the better the overall performance of the algorithm. Set the reference points for Supervolume calculation as (36.5, 14900), and the supervolumes of NSGA II, MOPSO, and modify MOPSO are 885.4, 3369.1, and 7247.6, respectively. It can be seen that the nondominated solution set obtained by modify PSO is the best.Figure 4
Multiobjective optimization results.Table 3
Objective function values.
Serial numberNSGA IIMOPSOModify MOPSOf1f2f1f2f1f2134.44721488933.86011481033.024314687234.48481488033.87311475633.15491466333455281486134.04401475133.295914656434.70181482834.52731474533.471814643534.74581482534.52901473733.655614614634.90181481934.57441472633.978814612735.56191478534.59041471434.119114602835.62411478034.73391469734.298614591935.83681477534.89291467834.4861145901036.04391476835.15631463634.733014560To sum up, the improved multiobjective PSO algorithm is applied to the multiobjective optimization problem of joint robot trajectory tracking fuzzy PID controller, and a better nondominated solution set can be obtained than the basic multiobjective PSO algorithm and NSGA II, which verifies the effectiveness and superiority of the algorithm in robot trajectory tracking fuzzy PID control optimization [24, 25].


## 5. Conclusion

In this study, a fuzzy PID controller is designed and applied to the trajectory tracking control of a six joint robot. Taking the controller output torque and trajectory tracking deviation as two optimization objectives, the improved multiobjective PSO algorithm is implemented to optimize the membership function of the fuzzy controller and optimize the adjustment of fuzzy rules. By comparing the improved multiobjective PSO algorithm with the basic multiobjective PSO algorithm and genetic algorithm applied to the multiobjective problem of joint robot trajectory tracking fuzzy PID controller, the results show that the nondominated solution set obtained by the improved PSO is better, which verifies the effectiveness and superiority of the algorithm applied to robot trajectory tracking fuzzy PID control optimization. The further research work is to continue to explore the effect of multiobjective PSO algorithm and other optimization algorithms (such as ant colony algorithm and neural network algorithm) to achieve the optimization of robot trajectory tracking controller, and to make a comparative analysis.



---

*Source: 3416682-2022-09-13.xml*