A speed control method for a launching mechanism based on adaptive fuzzy PID

Abstract

The present invention relates to the technical field of robot control, and particularly to a speed control method for a launching mechanism based on adaptive fuzzy PID, including constructing a mathematical model of the launching mechanism to obtain an approximate transfer function of the launching mechanism; designing a fuzzy controller for the launching mechanism, and a direct current brushless motor acts on the launching mechanism according to the output of the fuzzy PID controller, thereby controlling the speed of the projectiles launched by the launching mechanism. In the present invention, since part of the kinetic energy of the motor is transferred to the bullet when firing, it will cause a short-term speed drop of the motor, and because the environmental humidity will affect the friction coefficient of the friction wheel, for example, when continuously firing, the temperature of the friction wheel rises, and the friction wheel undergoes thermal expansion, resulting in a change in the friction coefficient, so that even if the motor rotates at a constant speed, it cannot launch the bullet at a constant speed. In the prior art, there is a lack of a stable mathematical model for the motor speed and the bullet launching speed, and there will be an unstable phenomenon of the speed of the projectiles launched by the launching mechanism in ordinary PID control.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly to a speed control method for a launching mechanism based on adaptive fuzzy PID. Background Art

[0002] A mobile robot is a branch in the field of robots that has a moving mechanism. It has a wide range of application fields, strong adaptability and purpose, and is currently a mainstream in robot design. However, there is less research on combat robots at present, and general combat robots have greater lethality and higher requirements for the accuracy and stability of strikes. In the face of a complex working environment, it is very easy to cause harm to innocent people or fail to focus on the target. Moreover, the automatic aiming system has extremely high scientific research value in many fields such as robot control, environmental perception, visual images, and precise strikes. Especially for the underactuated, multi-coupled, and non-linear factors existing in the automatic aiming process, such as uneven rotation speed of the launching motor and unstable thermal expansion of the friction wheel material under complex disturbances and the increasing requirements for shooting stability and accuracy, it is very difficult for traditional PID control to achieve a satisfactory control effect. Therefore, it is of great significance to carry out research on advanced control theories.

[0003] The launching mechanism of the omnidirectional mobile robot in the present invention is composed of two friction wheels wrapped around the motor with materials such as polyurethane. The bullet is launched by the high-speed rotation of the friction wheels. In order to launch an object at a constant speed, it is required that the two friction wheel motors rotate at the same and constant speed. However, since part of the kinetic energy of the motor will be transferred to the bullet when the bullet is launched, it will cause a short-term speed drop of the motor. And because the environmental humidity will affect the friction coefficient of the friction wheel, for example, when continuously launching, the temperature of the friction wheel rises, and the friction wheel undergoes thermal expansion, resulting in a change in the friction coefficient, so that the motor cannot launch the bullet at a constant speed even when rotating at a constant speed. Therefore, how to adjust the non-linear relationship between the rotation speed of the friction wheel motor and the initial velocity of the projectile is a technical problem that urgently needs to be solved in the current field. Summary of the Invention

[0004] Problems in the prior art: Since part of the kinetic energy of the motor will be transferred to the bullet when the bullet is launched, it will cause a short-term speed drop of the motor. And because the environmental humidity will affect the friction coefficient of the friction wheel, for example, when continuously launching, the temperature of the friction wheel rises, and the friction wheel undergoes thermal expansion, resulting in a change in the friction coefficient, so that the motor cannot launch the bullet at a constant speed even when rotating at a constant speed. There is a lack of a stable mathematical model for the motor rotation speed and the bullet launching speed in the prior art, and there is an unstable phenomenon of the projectile speed launched by the launching mechanism in ordinary PID control.

[0005] The technical solution adopted by the present invention is: A speed control method for a launching mechanism based on adaptive fuzzy PID includes the following steps:

[0006] S1. Construct a mathematical model of the launching mechanism to obtain an approximate transfer function of the launching mechanism;

[0007] Furthermore, the transfer function of the launching mechanism is obtained by calculating the product of the transfer function of the friction wheel speed and voltage and the transfer function of the projectile velocity when leaving the friction wheel and the friction wheel speed.

[0008] Furthermore, the transfer function of the friction wheel speed and voltage is obtained by mechanism modeling through the voltage balance equation and the torque balance equation.

[0009] Furthermore, the voltage balance equation and the torque balance equation are as follows:

[0010]

[0011] Among them, C mφ , C eφ respectively represent the torque electromagnetic constant and the electromagnetic induction constant; U a , E a , I a are the armature voltage, the motor armature back electromotive force, and the current respectively; L a is the motor armature inductance; R a is the equivalent resistance of the armature winding; M a , M L are the electromagnetic torque and the total load torque respectively; J is the moment of inertia; ω is the angular velocity of the motor.

[0012] Furthermore, the transfer function formula of the friction wheel speed and voltage is:

[0013]

[0014] Among them, C mφ represents the torque electromagnetic constant, J is the moment of inertia, L a is the motor armature inductance; R a is the equivalent resistance of the armature winding.

[0015] Furthermore, the formula for the transfer function of the projectile velocity when leaving the friction wheel and the friction wheel speed:

[0016]

[0017] Among them, V is the initial velocity of the projectile when leaving the friction wheel, ω is the angular velocity of the friction wheel, N is the distance between the centers of the two friction wheels, L is the displacement of the projectile from contacting the friction wheel to leaving the friction wheel, and R is the radius of the friction wheel.

[0018] Furthermore, the approximate transfer function of the launching mechanism, that is, the transfer function formula of the projectile velocity when leaving the friction wheel and the motor voltage is:

[0019]

[0020] Among them, J is the moment of inertia, and L a is the armature inductance of the motor; R a is the equivalent resistance of the armature winding; N is the distance between the centers of the two friction wheels, L is the displacement of the projectile from contacting the friction wheel to leaving the friction wheel, and R is the radius of the friction wheel.

[0021] S2. Design a fuzzy controller for the launching mechanism, including fuzzification, fuzzy inference, and defuzzification. The DC brushless motor acts on the launching mechanism composed of the DC brushless motor and the polyurethane friction wheel according to the output of the fuzzy PID controller, so as to control the speed of the projectile launched by the launching mechanism;

[0022] Furthermore, a two-dimensional fuzzy structure is adopted for the fuzzy control of the adaptive PID controller. The error e(t) between the given reference quantity V * (t) and the feedback value V(t), as well as the rate of change of the error are used as the two inputs of the fuzzy controller. After passing through the quantization factors K e and K ec , the input quantities are transformed from the basic domain to the fuzzy domain. The input variables are fuzzified, fuzzy-inferred, and the output variables are defuzzified in sequence, and are clarified through K up , K ui , and K ud respectively to obtain the PID parameter adjustment amounts ΔK p , ΔK i , and ΔK d ; finally, the PID parameter adjustment amounts are superimposed with the original PID parameters to obtain the final PID control parameters.

[0023] Advantages of the present invention:

[0024] 1. Mechanism modeling is carried out according to the voltage balance equation and the torque balance equation, and then the motion states of the friction wheel and the projectile are analyzed. Finally, the transfer function of the speed of the projectile leaving the friction wheel of the launching mechanism and the motor voltage is obtained, constructing a stable mathematical model for the launching mechanism;

[0025] 2. The nonlinear relationship between the rotational speed of the friction wheel and the initial velocity of the projectile is better adjusted through the adaptive fuzzy PID algorithm, solving the problem that the bullet cannot be stably launched. Compared with the ordinary PID algorithm, the speed of the bullet can also tend to be stable faster. Even after adding perturbations, the adaptive PID can better control the initial velocity of the bullet. Description of the drawings

[0026] Figure 1 is the model diagram of the launching mechanism of the present invention;

[0027] Figure 2 Schematic diagram of the relationship between the final velocity of the projectile and the tangential velocity of the friction wheel in the present invention;

[0028] Figure 3 Overall block diagram of the launching mechanism based on fuzzy PID velocity control in the present invention;

[0029] Figure 4 Simulink simulation diagram of the fuzzy PID of the launching mechanism in the present invention;

[0030] Figure 5 Fuzzy PID controller module in the present invention;

[0031] Figure 6 Initial velocity control signal diagram in the present invention;

[0032] Figure 7 Diagram of adding a small perturbation signal to the initial velocity control in the present invention. Detailed implementation manners

[0033] The present invention will be further described below in conjunction with the accompanying drawings and embodiments. This figure is a simplified schematic diagram, which only illustrates the basic structure of the present invention in a schematic manner. Therefore, it only shows the components related to the present invention.

[0034] As shown in Figure 1 is a schematic diagram of the structure of the launching mechanism. Above is the ammunition magazine, which is used to store the projectiles required for launching; there is a dial below the ammunition magazine, which is responsible for dialing the projectiles into the feeding link. Each time it rotates one grid, one projectile is dialed in; the ammunition magazine is equivalent to the magazine of a gun, and the dial is equivalent to the trigger. The projectile is dialed to the feeding link and slides onto the friction wheel. Two friction wheels rotating at high speed rotate in different directions to squeeze the projectile and launch it out of the barrel; however, since part of the kinetic energy of the motor will be transferred to the bullet when firing, it will cause a short-term speed drop of the motor, and since the environmental humidity will affect the friction coefficient of the friction wheel, for example, when continuously firing, the temperature of the friction wheel rises, and the friction wheel undergoes thermal expansion, resulting in a change in the friction coefficient, so that the motor cannot launch the bullet at a constant speed even when rotating at a constant speed.

[0035] A method for controlling the speed of a launching mechanism based on adaptive fuzzy PID includes the following steps:

[0036] S1. Establish the transfer function of the launching mechanism;

[0037] Mechanism modeling is carried out through the voltage balance equation and the torque balance equation, and its dynamic equation is as follows:

[0038]

[0039] Among them, C mφ , C eφ respectively represent the torque electromagnetic constant and the electromagnetic induction constant; Ua , E a , I a are the armature voltage, the back electromotive force of the motor armature, and the current respectively; L a is the armature inductance of the motor; R a is the equivalent resistance of the armature winding; M a , M L are the electromagnetic torque and the total load torque respectively; J is the moment of inertia; ω is the angular velocity of the motor;

[0040] The transfer function of the friction wheel speed and voltage is obtained as

[0041] Establish the dynamic mathematical model of the friction wheel angular velocity and the initial velocity of the projectile. Since the friction wheel launching system consists of two high-speed rotating friction wheels, they rotate in opposite directions, and the gap between them is slightly smaller than the diameter of the projectile. When the motor rotates at high speed, the two friction wheels rotate at high speed in opposite directions. When the projectile slowly enters between the two friction wheels, it is affected by the frictional force of the friction wheels and is ejected with a certain initial velocity. The schematic diagram of its launching principle Figure 2 is shown as follows;

[0042] By analyzing the motion state of the projectile during the launching process, when the velocity V X of the projectile along the tangent direction of the contact position of the friction wheel is equal to the linear velocity V x of the friction wheel, that is, V X = V x , there is no relative sliding between the projectile and the friction wheel; according to the geometric relationship, the relationship between the velocity V of the projectile when it leaves the friction wheel and the linear velocity V x of the friction wheel at this time is: where α is the angle between the line connecting the centers of the two friction wheels and the line connecting the center of the friction wheel and the center of the projectile. From V x = ωR, taking the Laplace transform on both sides simultaneously, the transfer function of the velocity of the projectile when it leaves the friction wheel and the rotational speed of the friction wheel is obtained:

[0043]

[0044] where V is the velocity of the projectile when it leaves the friction wheel, V x is the linear velocity of the friction wheel, ω is the angular velocity of the friction wheel, N is the distance between the centers of the two friction wheels, L is the displacement of the projectile from contacting the friction wheel to leaving the friction wheel, and R is the radius of the friction wheel.

[0045] Integrate the transfer function of the friction wheel speed and voltage and the velocity model of the projectile when it leaves the friction wheel to obtain the transfer function of the velocity of the projectile when it leaves the friction wheel of the launching mechanism and the motor voltage:

[0046]

[0047] Among them, the moment of inertia J = 1.84 kg·mm 2 , the armature inductance L of the motor a = 25.9 μH, the equivalent resistance R of the armature winding a = 181.5 mΩ, the torque electromagnetic constant C mφ = 10.9 mN·m / A, the distance N between the centers of the friction wheels is 214 mm, the projectile displacement L = 46.5 mm, the radius R of the friction wheel is 101 mm. Substituting these values into formula (3), the transfer function is obtained:

[0048]

[0049] S2. Design a fuzzy controller for the launching mechanism, including fuzzification, fuzzy inference, and defuzzification. The DC brushless motor acts on the launching mechanism composed of the DC brushless motor and the polyurethane friction wheel according to the output of the fuzzy PID controller, thereby controlling the speed of the projectile launched by the launching mechanism;

[0050] Fuzzification is to perform scale transformation on the precise input quantity of the fuzzy controller, transform it into its respective domain of discourse, and then obtain the fuzzy set represented by the membership function corresponding to the input quantity.

[0051] Fuzzy inference is the core part of the fuzzy controller, with the ability to simulate human reasoning based on fuzzy concepts, just like the human brain center; it mainly takes the established fuzzy inference rules as the basic conditions, uses the concepts of fuzzy sets and fuzzy relations to perform fuzzy relation synthesis, and performs fuzzy inference calculations on the input fuzzy quantity to obtain the fuzzy set of the output quantity.

[0052] Defuzzification is the inverse process of fuzzification, which converts the fuzzy output control quantity obtained by fuzzy inference into a precise control quantity that can act on the actual control system. Commonly used defuzzification methods include: the maximum membership degree method, the median method, the centroid method, etc.

[0053] Adopt a two-dimensional fuzzy structure to perform fuzzy control of the adaptive PID controller. Use the error e(t) between the given reference quantity V * (t) and the feedback value V(t), as well as the rate of change of the error as the two inputs of the fuzzy controller. Among them, V * (t) is the expected speed of the projectile, V(t) is the actual speed of the projectile, and the actual speed of the projectile is obtained by measuring the speed with the photoelectric gate on the launching mechanism. First, through the quantization factors K e , K ec transform the input quantity from the basic domain of discourse to the fuzzy domain of discourse, and perform three operation steps of fuzzification, fuzzy inference, and defuzzification of the input variables in sequence; the three output fuzzy variables respectively pass through K up , K ui , Kud Clarify it to obtain the final PID parameter adjustment amount ΔK p 、ΔK i 、ΔK d ; Finally, superimpose the PID parameter adjustment amount on the original PID parameters that have been tuned to generate the adaptive fuzzy PID control block diagram as shown in Figure 3 ;

[0054] The basic formula for realizing the self - adjustment of parameters in the adaptive fuzzy PID controller is as follows:

[0055]

[0056] Perform simulink simulation in MATLAB. As shown in Figure 4 , the simulink simulation diagram of the fuzzy PID of the launching mechanism is shown. The upper part is the simulation diagram of the second - order fuzzy PID, and the lower part is the simulation diagram of the ordinary PID. The input of the simulation diagram is the ideal speed of the bullet, and a certain disturbance is added to this input. The controlled object is the transfer function obtained from the motor and the launching mechanism in S1, and the output is the actual speed of the bullet. The actual speed passes through negative feedback to obtain the error from the ideal speed, and then through the tuning of the fuzzy PID or the ordinary PID, the final speed output is obtained.

[0057] In this embodiment, the quantization domain values of e and ec of the system are both [-3, 3]; the quantization domain values of the correction values ΔK p 、ΔK I 、ΔK D are [-0.3, 0.3], [-3, 3], [-3, 3] respectively, and are divided into seven levels: {-3, -2, -1, 0, 1, 2, 3}; as shown in Figure 5 , the quantization factors Ke e and Kec ec are selected as 3 and 0.01 respectively, and the scale factors KΔK p 、KΔK I 、KΔK D of ΔK up 、ΔK ui 、ΔK ud are selected as 0.0014, 0.1747, 0 respectively. The fuzzy subsets corresponding to each variable are {NB, NM, NS, ZO, PS, PM, PB}, that is, {Negative Big, Negative Medium, Negative Small, Zero, Positive Small, Positive Medium, Positive Big}, and triangular membership functions are selected for each linguistic value. Set the fuzzy rule table through expert experience and long - term test results.

[0058] Among them, the fuzzy rule table of ΔK P is as follows:

[0059]

[0060] ΔK I The fuzzy rule table of ΔK is as follows:

[0061]

[0062] ΔK D The fuzzy rule table of ΔK is as follows:

[0063]

[0064] The designed system is simulated and verified based on MATLAB / SIMULINK;

[0065] When building the simulation diagram in SIMULINK, setting the running time to 0.05 s and the speed set value to 10 m / s, the overshoot under ordinary PID control is about 17%, while the overshoot under fuzzy PID control is only 1.7%, almost without overshoot. As Figure 6 shown, to verify the adaptability of the fuzzy PID control method under external environmental interference, a small disturbance is added at 0.025 s. It can be seen that the fluctuation generated after 0.025 s under the fuzzy PID control method is much smaller than that under ordinary PID control; as Figure 7 shown, through comparison, it can be seen that the overshoot of the system under fuzzy PID control is much smaller than that under ordinary PID control, and there is an improvement in the response speed, and it has a certain adaptability to external interference.

[0066] Inspired by the above ideal embodiments of the present invention, through the above description, relevant staff can completely make various changes and modifications without departing from the technical idea of the present invention. The technical scope of the present invention is not limited to the content in the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A speed control method for a launching mechanism based on adaptive fuzzy PID, characterized in that, It includes the following steps: S1. Construct a mathematical model of the launching mechanism to obtain an approximate transfer function of the launching mechanism; The approximate transfer function of the launching mechanism is obtained by calculating the product of the transfer function of the friction wheel speed and voltage and the transfer function of the projectile velocity when leaving the friction wheel and the friction wheel speed; The formula for the transfer function of the projectile velocity when leaving the friction wheel and the friction wheel speed is: where V is the initial velocity of the projectile when leaving the friction wheel, ω is the angular velocity of the friction wheel, N is the distance between the centers of the two friction wheels, L is the displacement of the projectile from contacting the friction wheel to leaving the friction wheel, and R is the radius of the friction wheel; The formula for the approximate transfer function of the launching mechanism is: Among them, J is the moment of inertia, and L a is the armature inductance of the motor; R a is the equivalent resistance of the armature winding; N is the distance between the centers of the two friction wheels, L is the displacement of the projectile from contacting the friction wheel to leaving the friction wheel, and R is the radius of the friction wheel; S2. Design a fuzzy controller for the launching mechanism. The DC brushless motor acts on the launching mechanism according to the output of the fuzzy PID controller, thereby controlling the velocity of the projectile launched by the launching mechanism.

2. The speed control method of the launching mechanism based on adaptive fuzzy PID according to claim 1, wherein: The transfer function of the friction wheel speed and voltage is obtained by mechanism modeling through the voltage balance equation and the torque balance equation, and then analyzing the motion states of the friction wheel and the projectile to obtain the transfer function of the projectile initial velocity and the motor voltage.

3. The speed control method of the launching mechanism based on adaptive fuzzy PID according to claim 2, characterized in that, The formulas for the voltage balance equation and the torque balance equation are: Among them, C mφ , C eφ represent the torque electromagnetic constant and the electromagnetic induction constant respectively; U a , E a , I a are the armature voltage, the back electromotive force of the motor armature and the current respectively; L a is the armature inductance of the motor; R a is the equivalent resistance of the armature winding; M a , M L are the electromagnetic torque and the total load torque respectively; J is the moment of inertia; ω is the angular velocity of the motor.

4. The speed control method of the launching mechanism based on adaptive fuzzy PID according to claim 3, wherein, The formula for the transfer function of the friction wheel speed and voltage is: Among them, C mφ represents the torque electromagnetic constant; J is the moment of inertia; L a is the armature inductance of the motor; R a is the equivalent resistance of the armature winding.

5. The speed control method of the launching mechanism based on adaptive fuzzy PID according to claim 1, characterized in that The step S2 includes: performing fuzzy control of the adaptive PID controller by using a two-dimensional fuzzy structure, taking the error e(t) between the given reference quantity V * (t) and the feedback value V(t), and the change rate e(t) of the error as the two inputs of the fuzzy controller. After passing through the quantization factors K e , K ec , the input quantities are transformed from the basic universe of discourse to the fuzzy universe of discourse. The input variables are fuzzified, fuzzy inference is performed, and the output variables are defuzzified in sequence. After passing through K up , K ui , K ud , the crispification is performed to obtain the PID parameter adjustment amounts ΔK p , ΔK i , ΔK d ; finally, the PID parameter adjustment amounts are superimposed on the original PID parameters to obtain the final PID control parameters.

Images