CN105573121B - A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment - Google Patents

Abstract

A self-adjusting self-weight balance mechanism control algorithm for force feedback equipment. By combining the improved simple particle optimization algorithm, PID controller and linear system superposition, it is used to control the balance block of the self-adjusting self-weight balance mechanism of force feedback interactive equipment in real time. The position on the balance mechanism realizes the self-adjustment control of the self-weight balance mechanism of the force feedback interactive device, and effectively reduces the influence of the gravity of the arm mechanism of the force feedback interactive device on the human hand. The present invention can automatically find accurate PID controller parameters, and the linear superposition simplifies the modeling method of the self-adjusting self-weight balance mechanism control system. In addition, in order to overcome the high-frequency vibration caused by the introduction of the differential signal in the PID controller, the differential signal in the controller A first-order low-pass filter connected in series on the item can smooth the vibration of the output response of the system and eliminate the vibration generated by the balance weight of the self-adjusting self-weight balance mechanism.

Description

A control algorithm for self-adjusting self-weight balance mechanism of force feedback equipment

technical field

The invention belongs to the technical field of human-machine contact and interaction, and relates to a control algorithm of a force feedback device for self-adjusting self-weight balance mechanism.

Background technique

In the force feedback interactive device, it is very important to compensate the gravity of the device. In the existing technology, the gravity compensation method of the force feedback device is divided into two types: active gravity compensation and passive gravity compensation. The compensation method is aimed at the series link mechanism, using the motor excitation device to compensate the gravity of each joint. Since the motor or the excitation device must provide a part of the torque to offset the gravity, the feedback force generated by the motor or the excitation device must be reduced, and it is also It makes the system control complicated and may lead to the instability of the system. For the series link mechanism, the passive mass counterweight gravity compensation adopted additionally increases the moment of inertia and friction of the force feedback device, which affects the dynamic response of the force feedback device system. Based on this, Li Chunquan invented a force feedback device that can automatically adjust the self-weight balance (authorized invention patent 201110229208.7). Automatic gravity compensation of the device.

In order to control the position of the balance weight on the balance bar in real time, after modeling the self-adjusting self-weight balance mechanism of the force feedback device, a PID (Proportion Integral Derivative) controller is used to control the position of the balance weight on the balance bar. Several classic PID tuning rules such as Ziegler-Nichols rule, Cohen-Coon method and IAE method have been used to obtain desired PID parameters. However, it is time-consuming to use these methods, and it is very difficult to obtain optimal or nearly optimal PID parameters. In view of this, some artificial intelligence algorithms, such as Genetic Algorithms (GA), Simulated Annealing (SA) algorithm and Particle Swarm Optimization (PSO) algorithm are used to adjust the parameters of PID control. GA is often faster than SA algorithm. The PSO algorithm is a global optimization algorithm that imitates the swarm intelligence behavior of bird predation. It is often used to find the global optimum. This method is similar to the GA algorithm. Both algorithms randomly generate initial solutions, and then find the global optimal solution through evolutionary iterations. The difference between the two is: compared with the GA algorithm, the PSO algorithm has no clear choice, crossover and mutation operations, and the iterative search process of the PSO algorithm only depends on the individual optimal value of the particles and the optimal value of all particles in all previous iterations. value, and then use these optimal values to update the particle information of the next iteration. Therefore, the PSO algorithm has fewer adjustment parameters, lower calculation cost, faster convergence speed and better robustness. Therefore, we adopted a modified simple particle optimization algorithm (Modified Simple Particle Swarm Optimization Algorithm) that we have proposed to adjust the parameters of the PID algorithm. Compared with the PSO algorithm, the improved simple particle optimization algorithm is simpler, easier to implement, faster convergence speed, higher convergence accuracy, better stability, and can obtain better convergence values in a shorter calculation time.

Contents of the invention

The purpose of the present invention is to propose a force feedback device self-adjusting self-weight balance mechanism control algorithm.

The present invention is achieved through the following technical solutions.

A self-adjusting self-weight balance mechanism control algorithm for force feedback equipment, characterized in that the steps are as follows:

Step 1: When the torque τ _d (S) generated by the gravity and friction of the balance weight of the self-weight balance mechanism of the force feedback device is 0 = 0, the motor armature voltage is established As the input, the transfer function of the control system of the self-adjusting self-weight balance mechanism with the motor rotation angle θ _ARC1 as the output; when the torque τ _d (S) ≠ 0 generated by the gravity and friction of the balance weight of the self-adjusting self-weight balance mechanism , to establish the transfer function of the control system of the self-adjusting self-weight balance mechanism, which takes _the voltage U′ _m (S) required by the DC motor to overcome the torque τ d (S) as the input, and the output torque τ _d (S) of the motor as the output.

Step 2: Using the principle of linear superposition, establish the overall control system function of the force feedback interactive equipment to independently adjust the self-weight balance mechanism, ^d θ _ARC1 is the expected rotation angle of the DC motor, and establish the expected position of the balance weight on the balance bar The functional relationship with the expected rotation angle ^d θ _ARC1 of the DC motor, the error between the actual rotation angle θ _ARC1 and the expected rotation angle ^d θ _ARC1 of the motor is used as the input of the overall control system of the self-adjusting self-weight balance mechanism, after adjustment by the improved simple particle optimization PID control algorithm , as the input voltage U _m (S) of the DC motor armature, where, U′ _m (S) is used to produce the required voltage to offset the gravity and friction torque τ _d (S) of the balance weight of the self-adjusting self-weight balancing mechanism, is the voltage required to drive the DC motor to the desired rotation angle ^d θ _ARC1 ; the controller continuously adjusts the difference between the actual rotation angle θ _ARC1 and the desired rotation angle ^d θ _ARC1 of the DC motor, thereby continuously changing the armature input voltage U _m ( S), so that the actual rotation angle θ _ARC1 gradually reaches the desired rotation angle ^d θ _ARC1 ; finally, the deceleration driving wheel of the DC motor drives the deceleration driven wheel, the deceleration driven wheel drives the driving small wheel, and the driving small wheel drives the balance weight to reach the expectation of the balance bar Location

Step 3: Design and improve the simple particle optimization PID control algorithm, which is used to control the desired position of the balance weight of the self-adjusting self-weight balance mechanism on the balance bar The ITAE (Integral of Time multiply by Absolute Error) index is selected as an index to measure whether the proportional gain K _p , integral gain K _i , and differential gain K _d of the PID (Proportion Integral Derivative) controller in the improved simple particle optimization PID control algorithm are optimal. . Among them, the MSPSO (Modified Simple Particle Swarm Optimization) algorithm is used to optimize the proportional gain K _p , integral gain K _i and differential gain K _d parameters of PID control. The specific steps are as follows:

(1) In the improved simple particle optimization algorithm, Y=(Y ¹ ,Y ² ,…Y ⁿ ) means that n particles form a particle swarm; where, Y represents the set of particle swarms, and Y ⁱ (i=1,2 ,...,n) represent the position of the i-th particle in the particle swarm; the flight position search space of each particle in the particle swarm is a three-dimensional vector space, and each dimension in the three-dimensional position search space represents the proportion of the PID controller Gain K _p , integral gain K _i and differential gain K _d ; assuming that the total number of particle iterations is N times, then the position of the i-th particle in the k-th iteration is expressed as: in, and They are the proportional gain, integral gain, and differential gain of the i-th particle at the k-th iteration; these three parameters are random numbers, and their search intervals are all at a minimum value of 0 and a maximum value of ρ (ρ>0) within the interval.

(2) is the position of particle i in the kth iteration of the particle swarm, respectively and Substituting in as the corresponding proportional gain, integral gain, and differential gain in the PID controller, taking τ _d (S) and ^d θ _ARC1 (S) as the input excitation signal, in the kth iteration, its corresponding ITAE index is used as the MSPSO algorithm fitness value.

(3) According to all the fitness values ITAE of all n particles in the particle swarm from the first evolutionary iteration to the kth evolutionary iteration, the global optimal fitness value can be found as: in, and Respectively represent the global optimal proportional gain, global optimal integral gain, and global optimal differential gain of all n particles in the particle swarm in k iterations.

(4)Y ⁱ (k+1)＝λ ₁ η ₁ Y ⁱ (k)+λ ₂ η ₂ [P ^g (k)-Y ⁱ (k)] is used as an evolution iteration update equation, which means that among all n particles The i-th particle of is updated at the k+1 iteration; among them, η _i (i=1, 2) represent random numbers between 0 and 1, and λ ₁ and λ ₂ represent the cognitive learning rate and social Learning rate, the two generally take a constant value of 1.49445.

(5) Boundary condition agreement: A particle swarm composed of n particles, after the k-th iterative calculation, the position range of a certain dimension in the position of the i-th particle X ⁱ (k) exceeds the specified interval: the minimum of the interval The value is 0, and the maximum value is ρ (ρ>0). Then, a certain dimension of the position of the i-th particle Y ⁱ (k) must be reassigned to 0 or ρ (ρ>0). If the position variable of this dimension is less than 0, then the position variable of this dimension is designated as 0; if the position variable of this dimension is greater than ρ (ρ>0), then the position variable of this dimension is designated as ρ (ρ>0).

(6) If the operation reaches the maximum number of iterations, the iteration is ended, and the global optimal proportional gain, global optimal integral gain, and global optimal differential gain after all evolutionary iterations are found. Otherwise, go back to step (2) and continue to execute sequentially. Finally, we can find out the proportional gain, integral gain and derivative gain of optimal PID control.

Step 4: In order to overcome the high-frequency interference introduced by the differential signal in the PID controller, the Laplace transform of the PID controller A first-order low-pass filter is connected in series with the differential term in The vibration of the smooth system output response, where 0<Ξ<1.

The advantages of the present invention are: the improved simple particle optimization algorithm is combined with the PID controller, and an improved simple particle optimization PID control algorithm is invented, which can automatically find accurate PID controller parameters, and the linear superposition principle is used to simplify autonomous Adjust the modeling method of the self-weight balance mechanism control system. In addition, in order to overcome the high-frequency vibration caused by the introduction of the differential signal in the PID controller, a first-order low-pass filter is connected in series with the differential term of the controller to smooth the system output response. vibration, eliminating the vibration effect produced by the self-adjusting balance weight of the self-weight balance mechanism.

Description of drawings

Fig. 1 is a connection diagram of the self-adjusting self-weight balance mechanism of the force feedback device, which is the control object of the present invention. In the figure: 1 is a balance bar, 2 is a balance weight, 3 is a wire rope, 4 is a connecting piece, 5 is a small driving wheel, 6 is a deceleration driving wheel, 7 is a deceleration driven wheel, 8 is a DC motor, and 9 is a photoelectric encoder .

Fig. 2 is a schematic diagram of the dynamic analysis of the self-adjusting self-weight balance mechanism of the force feedback device.

Fig. 3 is the servo transmission principle of the DC motor 8 in Fig. 1 .

Fig. 4 is a schematic diagram of self-adjusting self-weight balance position control in Fig. 2 .

Figure 5 is a schematic diagram of the self-adjusting self-weight balance position control based on the improved simple particle optimization PID controller.

Detailed ways

The present invention will be further described in conjunction with accompanying drawings.

As shown in Figure 1, the self-adjusting self-weight balance mechanism consists of a balance bar 1, a balance weight 2, a steel wire rope 3, a connecting piece 4, a small driving wheel 5, a deceleration driving wheel 6, a deceleration driven wheel 7, a DC motor 8 and a photoelectric encoder 9 constitute. The balance weight 2 is installed on the balance pole 1 , the contact between the two is very smooth, the friction is very small, and the balance weight 2 can move along the two directions of the balance pole 2 . The balance pole 1 is fixed on the force feedback device through the connecting piece 4 . The balance weight drives the deceleration driving wheel 6 to be installed on the DC motor 8 and rotates coaxially with the DC motor 8. The balance weight drives the deceleration driving wheel 6 to drive the balance weight to drive the deceleration driven wheel 7 to move through the steel wire rope, and the balance weight drives the deceleration driven wheel 7 to drive Steamboat 5 rotates coaxially with it.

For the convenience of description, the self-adjusting self-weight balance mechanism of the force feedback device in Fig. 1 is simplified to the dynamic analysis diagram of the self-adjusting self-weight balance mechanism in Fig. 2 .

As shown in Figure 2, the mass of the balance weight 2 is m _b , the gravity is G _b , the force exerted by the wire rope 3 on the balance weight is Fp, and the friction force is Ff ₃ , the balance weight 2 is on the balance bar 1 The instantaneous position at distance o ₂ is l _b , and the instantaneous velocity is Instantaneous acceleration The dynamic equation of the balance weight 2 sliding in the direction of Qo ₂ and o ₂ Q on the balance pole 1 is:

The ratio of the radius r ₁ of the reduction driving wheel 6 to the radius r ₂ of the reduction driven wheel 7 is 1/N, and the turning radius of the small driving wheel 5 is r ₃ . The deceleration driving wheel 6 is nested on the rotating shaft of the DC motor 8, and rotates coaxially with the rotating shaft of the DC motor 8, regarded as a whole, and their rotation angles are the same, denoted as θ _ARC1 . The rotation angle of the small driving wheel 5 and the reduction driven wheel 7 also rotate coaxially, and they are regarded as a whole, and the rotation angle is also the same, and the rotation angle is recorded as θ _ARC2 . The mathematical relationship between the rotation angles θ _ARC1 and θ _ARC2 is:

The driving small wheel 5 pulls the balance weight 2 to move on the balance pole 1 through the steel wire rope 3, the position of the balance weight 2 on the balance pole is recorded as l _b , the mathematical relationship between the rotation angle θ _ARC1 of the DC motor 8 and the rotation angle θ _ARC2 of the driving small wheel 5 for:

The DC motor 8 and the deceleration driving wheel 6 are taken as a whole, and their moment of inertia is J ₁ . The deceleration driven wheel 7 and the driving small wheel 5 rotate coaxially, and can also be regarded as a whole, and its moment of inertia is J ₂ .

Taking the deceleration driven wheel 7 and the driving small wheel 5 as a whole, and carrying out force analysis, it can be obtained

Among them, _τ2 is the acting moment of the DC motor 8 and the reduction driving wheel 6 on the reduction driven wheel 7 and _the driving small wheel 5 as a whole; The acting force of the block 2 on the driving small wheel 5 and the deceleration driven wheel 7 through the steel wire rope 3 is ^c Fp, and this force and Fp are acting force and reaction force each other.

As shown in Figure 3, the DC motor 8 and the deceleration driving wheel 6 are analyzed as a whole. According to Kirchhoff's theorem, the relationship between the armature control DC motor is as follows:

Among them, U _m , I _m , L _m and R _m represent the voltage, current, inductance and resistance in the armature circuit of the DC motor 8 respectively, and K _e is called the potential constant of the DC motor 8 .

The relationship between the output torque _τ1 of the DC motor 8 and the armature circuit current _Im is:

τ ₁ =K ₁ I _m (7)

Wherein, K ₁ is the torque constant of the DC motor 8 .

Considering the mechanical characteristics of the DC motor 8, according to Newton's law, the torque equation of the DC motor 8 can be obtained:

Among them, J ₁ and B ₁ represent the moment of inertia and viscous friction coefficient of the DC motor 8 rotor, respectively. Substituting equations (1)-(5) into equation (8), the differential equation between the output torque τ1 of the DC motor ₈ and the motor rotation angle _θARC1 can be obtained:

Among them, J represents the total moment of inertia of the entire system equivalent to the DC motor 8, B represents the total viscous friction coefficient of the entire system equivalent to the DC motor 8, and τ _d represents the force on the balance weight of the self-adjusting self-weight balance mechanism. The torque produced by gravity and friction (or expressed as the torque required to counteract the gravity and friction that the balance weight 2 is subjected to when moving) is expressed as follows respectively:

The required motor rotation angle θ _ARC1 can be obtained by controlling the output torque τ ₁ of the DC motor 8, and then according to formulas (3) and (4), l _b = r ₃ θ _ARC1 /N can be obtained, so that the balance weight 2 can be controlled on the balance bar position l _b on 1.

Laplace transform of equations (7), (8) and (9) can be obtained as (10), (11) and (12):

U _m (S) = I _m (S) R _m + SL _m I _m (s) + SK _e θ _ARC1 (S) (10)

Wherein, U _m (S), I _m (S) and θ _ARC1 (S) represent the Laplace transforms of U _m , I _m and θ _ARC1 respectively.

τ ₁ (S) = K ₁ I _m (S) (11)

Among them, τ ₁ (S) represents the Laplace transform of τ ₁ .

τ ₁ (S)＝S ² Jθ _ARC1 (S)+SBθ _ARC1 (S)+τ _d (S) (12)

Among them: τ _d (S) represents the Laplace transform of τ _d .

Substituting equations (11) and (12) into equation (10) to sort out, we can get:

When τ _d (S) = 0, at this time, Rearranging formula (13), the transfer function of the control system for self-adjusting self-weight balance mechanism can be obtained as follows:

When the torque τ _d (S) ≠ 0, the DC motor 8 produces the voltage value U′ _m (S) required to overcome the gravity and friction torque τ _d (S) of the balance weight 2, thus establishing U The input-output relationship between ′ _m (S) and τ _d (S) is:

Combining Equation (14) and Equation (15), and adopting the principle of linear superposition, the overall control system of the self-adjusting self-weight balance mechanism can be constructed, as shown in Figure 4. ^d θ _ARC1 is the expected rotation angle of the DC motor 8, and the expected position of the balance weight can be calculated according to formula (4) Convert to the desired joint angle ^d θ _ARC1 . In this way, the error between the actual rotation angle θ _ARC1 of the motor 8 and the expected rotation angle ^d θ _ARC1 is used as the input of the controller, and U _m (S) is used as the output of the controller to provide the input voltage of the armature of the DC motor 8 for driving the DC motor continuously. Approach to the desired rotation angle ^d θ _ARC1 . In this way, the controller continuously adjusts the difference between the expected rotation angle ^d θ _ARC1 and the actual rotation angle θ _ARC1 of the DC motor 8, thereby continuously changing the armature input voltage U _m (S), so that the actual rotation angle θ _ARC1 gradually reaches the desired rotation angle ^d θ _ARC1 . Finally, the DC motor 8 drives the deceleration driving wheel 6, the deceleration driving wheel 6 drives the deceleration driven wheel 7 through the wire rope, the deceleration driven wheel 7 drives the driving small wheel 5 to rotate coaxially, the driving small wheel 5 drives the steel wire rope 3, and the steel wire rope 3 drives the balance weight 2 Arrive at the desired position of the balance pole 1

In Fig. 4, the DC servo control system of the self-adjusting self-weight balance mechanism is a linear system, and a PID (proportional-differential-integral) controller is used for position control. The Laplace transform of the PID controller is:

Among them, K _p is the proportional gain, K _i is the integral gain, and K _d is the differential gain.

In order to select reasonable PID controller parameters, we invented an improved simple particle optimization PID controller, which uses the improved simple particle algorithm to intelligently adjust the PID parameters, and is used to efficiently control the balance block of the self-adjusting self-weight balance mechanism. The principle is shown in the figure 5. The ITAE (Intergral of Time multiply by Absolute Error) index is selected as the evaluation index to measure whether the proportional gain K _p , integral gain K _i , and differential gain K _d of the PID controller are optimal. The index is defined as follows:

Among them, e(t) represents the error between output and input in the control system, and t is a time variable.

The specific optimization adjustment steps are as follows:

(1) In the improved simple particle optimization algorithm, Y=(Y ¹ ,Y ² ,…Y ⁿ ) means that n particles form a particle swarm; where, Y represents the set of particle swarms, and Y ⁱ (i=1,2 ,…,n) represent the position of the i-th particle in the particle swarm; the flight position search space of each particle in the particle swarm is a 3-dimensional vector space, and each dimension of the 3-dimensional position search space represents the PID controller’s Proportional gain K _p , integral gain K _i and differential gain K _d ; assuming that the total number of particle iterations is N times, then the position of the i-th particle in the k-th iteration is expressed as: in, and They are the proportional gain, integral gain, and differential gain of the i-th particle at the k-th iteration; these three parameters are random numbers, and their search intervals are all at a minimum value of 0 and a maximum value of ρ (ρ>0) within the interval.

(2) is the position of each particle i in the kth iteration of the particle swarm, respectively and Substitute in Figure 5 as the corresponding proportional gain, integral gain, and differential gain in the PID controller, and take τ _d (S) and ^d θ _ARC1 (S) as the input excitation signal. In the kth iteration, the corresponding ITAE index is as The fitness value of the improved simple particle optimization PID algorithm is calculated using formula (17).

(3) According to all the fitness values ITAE of all n particles in the particle swarm from the first evolutionary iteration to the kth evolutionary iteration, find the global optimal fitness value: in, and Respectively represent the global optimal proportional gain, global optimal integral gain, and global optimal differential gain of all n particles in the particle swarm in k iterations.

(4)Y ⁱ (k+1)＝λ ₁ η ₁ Y ⁱ (k)+λ ₂ η ₂ [P ^g (k)-Y ⁱ (k)] is used as an evolution iteration update equation, which means that among all n particles The i-th particle of is updated at the k+1 iteration; among them, η _i (i=1, 2) represent random numbers between 0 and 1, and λ ₁ and λ ₂ represent the cognitive learning rate and social Learning rate, the two generally take a constant value of 1.49445. .

(5) Boundary condition agreement: If a particle swarm composed of n particles, after the iterative calculation of the kth iteration, the position range of a certain dimension in the i-th particle position X ⁱ (k) exceeds the specified interval: The minimum value of this interval is 0, and the maximum value is ρ (ρ>0). Then a certain dimension of the position of the i-th particle ^Xi (k) must be reassigned to 0 or ρ (ρ>0). If the position variable of this dimension is less than 0, then the position variable of this dimension is designated as 0; if the position variable of this dimension is greater than ρ (ρ>0), then the position variable of this dimension is designated as ρ (ρ>0).

(6) If the number of evolution iterations reaches the maximum number of iterations N, then end the iteration, and find the global optimal proportional gain, global optimal integral gain, and global optimal differential gain after all evolution iterations. Otherwise, go back to step (2) and continue to execute sequentially. In this way, according to the improved simple particle optimization PID control algorithm in Fig. 5, the proportional gain, integral gain and differential gain of the optimal PID control of the system can be found out.

In order to overcome the high-frequency interference introduced by the differential signal in PID, the Laplace transform of the PID controller A first-order low-pass filter is connected in series on the differential term in The vibration of the smooth system output response, where 0<Ξ<1.

Claims (1)

1. A force feedback device self-adjusting self-weight balancing mechanism control algorithm, characterized in that as follows: Step 1: When the torque τ _d (S) generated by the gravity and friction of the balance weight of the self-weight balance mechanism of the force feedback device is 0 = 0, the motor armature voltage is established As the input, the transfer function of the control system of the self-adjusting self-weight balance mechanism with the motor rotation angle θ _ARC1 as the output; when the torque τ _d (S) ≠ 0 generated by the gravity and friction of the balance weight of the self-adjusting self-weight balance mechanism , establish the transfer function of the control system of the self-adjusting self-weight balance mechanism with the voltage U′ _m (S) required by the DC motor to overcome the torque τ _d (S) as the input and the output torque τ _d (S) of the motor as the output; Step 2: Using the principle of linear superposition, establish the overall control system function of the force feedback interactive equipment to independently adjust the self-weight balance mechanism, ^d θ _ARC1 is the expected rotation angle of the DC motor, and establish the expected position of the balance weight on the balance bar The functional relationship with the expected rotation angle ^d θ _ARC1 of the DC motor, the error between the actual rotation angle θ _ARC1 and the expected rotation angle ^d θ _ARC1 of the motor is used as the input of the overall control system of the self-adjusting self-weight balance mechanism, after adjustment by the improved simple particle optimization PID control algorithm , as the input voltage U _m (S) of the DC motor armature, where, U′ _m (S) is used to produce the required voltage to offset the gravity and friction torque τ _d (S) of the balance weight of the self-adjusting self-weight balancing mechanism, is the voltage required to drive the DC motor to the desired rotation angle ^d θ _ARC1 ; the controller continuously adjusts the difference between the actual rotation angle θ _ARC1 and the desired rotation angle ^d θ _ARC1 of the DC motor, and continuously changes the armature input voltage U _m (S ), so that the actual rotation angle θ _ARC1 gradually reaches the desired rotation angle ^d θ _ARC1 ; finally, the deceleration driving wheel of the DC motor drives the deceleration driven wheel, the deceleration driven wheel drives the driving small wheel, and the driving small wheel drives the balance weight, so that the balance bar reaches the desired Location Step 3: Design and improve the simple particle optimization PID control algorithm, which is used to control the desired position of the balance weight of the self-adjusting self-weight balance mechanism on the balance bar The ITAE index is selected as the index to measure whether the proportional gain K _p , the integral gain K _i and the differential gain K _d of the PID controller in the improved simple particle optimization PID control algorithm are optimal; in the improved simple particle optimization PID control algorithm, the improved The simple particle optimization algorithm is used to optimize the proportional gain K _p , integral gain K _i and differential gain K _d parameters of the control PID controller. The specific steps are as follows: (1) In the improved simple particle optimization algorithm, Y=(Y ¹ ,Y ² ,…Y ⁿ ) means that n particles form a particle swarm; where, Y represents the set of particle swarms, and Y ⁱ (i=1,2 ,...,n) represent the position of the i-th particle in the particle swarm; the flight position search space of each particle in the particle swarm is a three-dimensional vector space, and each dimension in the three-dimensional position search space represents the proportion of the PID controller Gain K _p , integral gain K _i and differential gain K _d ; assuming that the total number of particle iterations is N times, then the position of the i-th particle in the k-th iteration is expressed as: in, and are the proportional gain, integral gain, and differential gain of the i-th particle at the k-th iteration; these three parameters are random numbers, and their search ranges are all within the interval between the minimum value of 0 and the maximum value of ρ, where , ρ>0; (2) is the position of particle i in the kth iteration of the particle swarm, respectively and As the corresponding proportional gain, integral gain, and differential gain in the PID controller, taking τ _d (S) and ^d θ _ARC1 as the input excitation signal, in the kth iteration, its corresponding ITAE index is used as the adaptation of the improved simple particle optimization algorithm degree value; (3) According to all the fitness values ITAE of all n particles in the particle swarm from the first evolutionary iteration to the kth evolutionary iteration, the global optimal fitness value can be found as: in, and Respectively represent the global optimal proportional gain, global optimal integral gain, and global optimal differential gain of all n particles in the particle swarm in k iterations; (4)Y ⁱ (k+1)＝λ ₁ η ₁ Y ⁱ (k)+λ ₂ η ₂ [P ^g (k)-Y ⁱ (k)] is used as an evolution iteration update equation, which means that among all n particles The i-th particle of is updated at the k+1 iteration; among them, η _i (i=1, 2) represent random numbers between 0 and 1, and λ ₁ and λ ₂ represent the cognitive learning rate and social Learning rate, both take a constant value of 1.49445; (5) Boundary condition agreement: for a particle swarm composed of n particles, after the k-th iterative calculation, the position range of a certain dimension in the position Y ⁱ (k) of the i-th particle exceeds the specified interval: The minimum value is 0, and the maximum value is ρ, where ρ>0; then a certain dimension of the position of the i-th particle Y ⁱ (k) must be redesignated as 0 or ρ; if the position variable of this dimension is less than 0, Then the position variable of this dimension is designated as 0; if the position variable of this dimension is greater than ρ, then the position variable of this dimension is designated as ρ; (6) If the operation reaches the maximum number of iterations N, then end the iteration and find the global optimal proportional gain, global optimal integral gain, and global optimal differential gain after all evolutionary iterations; otherwise, return to step (2) to continue the sequence Execute; finally, find the proportional gain, integral gain and differential gain of the optimal PID control; Step 4: In order to overcome the high-frequency interference introduced by the differential signal in the PID controller, the Laplace transform of the PID controller A first-order low-pass filter is connected in series with the differential term in to form The vibration used to smooth the output response of the system, where 0<Ξ<1.