CN105573121B - A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment - Google Patents
A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment Download PDFInfo
- Publication number
- CN105573121B CN105573121B CN201610028359.9A CN201610028359A CN105573121B CN 105573121 B CN105573121 B CN 105573121B CN 201610028359 A CN201610028359 A CN 201610028359A CN 105573121 B CN105573121 B CN 105573121B
- Authority
- CN
- China
- Prior art keywords
- particle
- self
- gain
- arc1
- balance mechanism
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000007246 mechanism Effects 0.000 title claims abstract description 40
- 239000002245 particle Substances 0.000 claims abstract description 90
- 230000005484 gravity Effects 0.000 claims abstract description 20
- 230000004044 response Effects 0.000 claims abstract description 6
- 238000009499 grossing Methods 0.000 claims abstract description 5
- 230000002452 interceptive effect Effects 0.000 claims abstract description 5
- 230000010355 oscillation Effects 0.000 claims abstract description 3
- 238000005457 optimization Methods 0.000 claims description 24
- 230000009467 reduction Effects 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 9
- 238000000034 method Methods 0.000 claims description 8
- 230000005284 excitation Effects 0.000 claims description 6
- 230000009326 social learning Effects 0.000 claims description 4
- 230000013016 learning Effects 0.000 claims description 3
- 229910000831 Steel Inorganic materials 0.000 description 9
- 239000010959 steel Substances 0.000 description 9
- 238000010586 diagram Methods 0.000 description 4
- 230000001276 controlling effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000001149 cognitive effect Effects 0.000 description 2
- 230000014509 gene expression Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/041—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a variable is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment, by the way that the superposition for improving simple single-particle optimizing algorithm, PID controller and linear system is combined, position of the balance weight on balance mechanism for the autonomous adjusting dead weight balance mechanism for controlling force feedback interactive device in real time, it realizes the control of the autonomous adjusting dead weight balance mechanism of force feedback interactive device, effectively reduces caused influence of the arm mechanism gravity of force feedback interactive device on human hand.The present invention can be automatically found accurate PID controller parameter, linear superposition simplifies autonomous adjusting dead weight balance mechanism modeling of control system means, in addition, in order to overcome differential signal in PID controller to introduce caused high-frequency vibration, it connects on the differential term of controller low-pass first order filter, can smoothing system output response vibration, eliminate oscillation caused by the balance weight of autonomous adjusting dead weight balance mechanism.
Description
Technical Field
The invention belongs to the technical field of human-computer contact interaction, and relates to a control algorithm for an automatic self-adjusting self-weight balance mechanism of force feedback equipment.
Background
In the force feedback interaction device, gravity compensation is performed on the device, which is a very critical problem, in the prior art, gravity compensation methods of the force feedback device are divided into active gravity compensation and passive gravity compensation, wherein the active gravity compensation method is used for a serial link mechanism, a motor excitation device is used for compensating gravity of each joint, and since the motor or the excitation device needs to provide a part of torque for counteracting gravity, feedback force generated by the motor or the excitation device is necessarily reduced, and system control is also complicated, which may cause instability of the system. Aiming at the serial connection rod mechanism, the passive mass counterweight gravity compensation is adopted, so that the rotational inertia and friction of the force feedback equipment are additionally increased, and the dynamic response of a force feedback equipment system is influenced. Based on this, the lischun spring invented a force feedback device for self-adjusting self-weight balance (the issued patent of invention 201110229208.7), which realizes automatic gravity compensation for the force feedback device by controlling the position of the balance weight of the self-adjusting self-weight balance mechanism on the balance bar in real time.
In order to control the position of the balance weight on the balance rod in real time, after the self-adjusting self-weight balance mechanism of the force feedback device is modeled, a PID (proportion integration differentiation) controller is adopted to control the position of the balance weight on the balance rod. Several classical PID tuning rules such as the Ziegler-Nichols rule, the Cohen-Coon method, and the IAE method have been used to obtain the desired PID parameters. However, using these methods is time consuming and it is very difficult to obtain optimal or near optimal PID parameters. In view of this, some artificial intelligence Algorithms, such as Genetic Algorithms (GA), Simlated Analyzing (SA) and Particle Swarm Optimization (PSO) Algorithms, are used to adjust the parameters of PID control. GA is often faster than SA algorithm. The PSO algorithm is a global optimization algorithm simulating the intelligent behavior of the group predating birds, is commonly used for finding global optima, and is similar to the GA algorithm. Both algorithms randomly generate an initial solution and then through evolutionary iteration, find a globally optimal solution. The difference between the two is that: compared with the GA algorithm, the PSO algorithm has no explicit selection, intersection and mutation operation, and the iterative search process of the PSO algorithm only depends on the optimal values of the individual particles and the optimal values of the overall particles in all previous iterations, and the particle information of the next iteration is updated by using the optimal values. Therefore, the PSO algorithm has fewer adjusting parameters, lower calculation cost, higher convergence rate and better robustness. Therefore, we have adopted a Modified Simple Particle Optimization Algorithm (Modified Simple Particle Swarm Optimization Algorithm) we have proposed for adjusting the PID Algorithm parameters. Compared with the PSO algorithm, the improved simple particle optimization algorithm is simpler, easier to realize, higher in convergence speed, higher in convergence precision and better in stability, and a better convergence value can be obtained in shorter calculation time.
Disclosure of Invention
The invention aims to provide a control algorithm for an automatic self-adjusting self-weight balance mechanism of force feedback equipment.
The invention is realized by the following technical scheme.
A control algorithm for an automatic self-adjusting self-weight balance mechanism of force feedback equipment is characterized by comprising the following steps:
step 1: torque tau generated by gravity and friction of balance block of self-regulating self-weight balance mechanism when force feedback equipmentdWhen (S) is 0, the motor armature voltage is establishedFor input, the motor rotation angle theta is usedARC1A control system transfer function for the output self-regulating deadweight balancing mechanism; when the torque tau generated by the gravity and the friction force borne by the balance block of the self-adjusting self-weight balance mechanismdWhen (S) ≠ 0, establishing that a direct current motor overcomes taudVoltage U 'required for (S) moment'm(S) as input, using motor output torque taudAnd (S) is a control system transfer function of the output self-adjusting self-weight balance mechanism.
Step 2: adopting the linear superposition principle to establish the integral control system function of the self-adjusting self-weight balance mechanism of the force feedback interactive equipment,dθARC1is the desired rotation angle of the DC motor, and establishes the desired position of the balance weight on the balance barAngle of rotation desired for DC motordθARC1As a function of the actual rotational angle theta of the motorARC1And desired turning angledθARC1The error is used as the input of the integral control system of the self-adjusting self-weight balance mechanism, and is used as the input voltage U of the direct current motor armature after being adjusted by the improved simple particle optimization PID control algorithmm(S) wherein, in the above step,U′m(S) is a torque for generating a torque tau for counteracting the gravity and the friction of the balance weight of the self-adjusting weight balance mechanismd(S) a desired voltage of the voltage,is used for driving the direct current motor to rotate to a desired rotation angledθARC1The required voltage; the actual rotation angle theta of the direct current motor is continuously adjusted by the controllerARC1And desired turning angledθARC1Thereby continuously varying the armature input voltage Um(S) so that the actual rotation angle thetaARC1Gradually reaching the desired rotation angledθARC1(ii) a Finally, a speed reduction driving wheel of the direct current motor drives a speed reduction driven wheel, the speed reduction driven wheel drives a driving small wheel, and the driving small wheel drives a balance block to reach the expected position of the balance rod
And step 3: designing and improving a simple particle optimization PID control algorithm for controlling the expected position of a balance block of an automatic self-adjusting self-weight balance mechanism on a balance rodSelecting ITAE (Intergral of Time multiplex by Absolute Error) index as proportional gain K for measuring PID (probability integral deviation) controller in improved simple particle optimization PID control algorithmpIntegral gain KiDifferential gain KdWhether the optimal index is reached. Wherein, MSPSO (modified Simple Particle Swarm optimization) algorithm is used to optimize the proportional gain K of the control PIDpIntegral gain KiAnd a differential gain KdThe method comprises the following specific steps:
(1) in the improved simple particle optimization algorithm, Y ═ Y (Y)1,Y2,…Yn) Means that n particles constitute one particle group; wherein Y represents a set of particle groups, Yi(i ═ 1,2, …, n) represents the position of the ith particle in the population; each particle flight position search space of the particle swarm is a three-dimensional vector space, and each dimension in the three-dimensional position search space respectively represents the proportional gain K of the PID controllerpIntegral gain KiAnd a differential gain Kd(ii) a Assuming that the total number of particle iterations is N, then the position of the ith particle in the kth iteration is represented as:wherein,andproportional gain, integral gain and differential gain of the ith particle in the kth iteration are respectively obtained; these three parameters are random numbers and their search intervals range within an interval with a minimum value of 0 and a maximum value of p (p > 0).
(2)Is the position of particle i in the kth iteration of the particle swarm, respectivelyAndsubstituting into the corresponding proportional gain, integral gain and differential gain in the PID controller, and dividing tau intod(S) anddθARC1(S) as input excitation signal, and in the kth iteration, its corresponding ITAE index as fitness value of the MSPSO algorithm.
(3) According to all fitness values ITAE of all n particles in the particle swarm from the 1 st evolutionary iteration to the kth evolutionary iteration, the global optimal fitness value can be found as follows:wherein,andand respectively representing the global optimal proportional gain, the global optimal integral gain and the global optimal differential gain of all n particles in the particle swarm in k iterations.
(4)Yi(k+1)＝λ1η1Yi(k)+λ2η2[Pg(k)-Yi(k)]As an evolutionary iterative update equation, the ith particle in all the n particles is updated in the (k + 1) th iteration, wherein ηi(i ═ 1,2) respectively represent random numbers between 0 and 1, λ1And λ2Each representing a cognitive learning rate and a social learning rate, both of which are generally constant values 1.49445.
(5) And boundary condition agreement: after the particle swarm composed of n particles is subjected to the k iterative computation, the ith particle position Xi(k) Is out of the specified interval: the minimum value of this interval is 0 and the maximum value is ρ (ρ > 0). Then, firsti particles Yi(k) A certain dimension of the position must be reassigned to 0 or ρ (ρ > 0). If the dimension position variable is less than 0, the dimension position variable is designated as 0; the dimension position variable is greater than ρ (ρ > 0), then the dimension position variable is designated as ρ (ρ > 0).
(6) If the operation reaches the maximum iteration times, the iteration is ended, and the global optimal proportional gain, the global optimal integral gain and the global optimal differential gain after all evolutionary iterations are found. Otherwise, returning to the step (2) to continue the sequential execution. Finally, we can find out the proportional gain, integral gain and differential gain of the optimal PID control.
And 4, step 4: in order to overcome the high-frequency interference introduced by differential signals in the PID controller, Laplace transform in the PID controllerThe first order low-pass filter is connected in series with the differential term in the middleThe smoothing system outputs a vibration in response, wherein 0 < xi < 1.
The invention has the advantages that: the improved simple particle optimization algorithm is combined with a PID controller, an improved simple particle optimization PID control algorithm is invented, accurate PID controller parameters can be automatically found out through the algorithm, a linear superposition principle is used for simplifying a modeling means of a control system of the self-adjusting self-weight balance mechanism, in addition, in order to overcome high-frequency vibration caused by introduction of differential signals in the PID controller, a first-order low-pass filter is connected in series with a differential term of the controller and used for smoothing vibration of system output response, and an oscillation effect generated by a balance block of the self-adjusting self-weight balance mechanism is eliminated.
Drawings
Fig. 1 is a connection diagram of an autonomous adjusting weight balance mechanism of a force feedback device, which is a control object of the present invention. In the figure: the balance rod is 1, the balance block is 2, the steel wire rope is 3, the connecting piece is 4, the driving small wheel is 5, the speed reducing driving wheel is 6, the speed reducing driven wheel is 7, the direct current motor is 8, and the photoelectric encoder is 9.
Fig. 2 is a schematic view of a dynamic analysis of the self-adjusting deadweight balancing mechanism of the force feedback device.
Fig. 3 shows the servo drive principle of the dc motor 8 of fig. 1.
Fig. 4 is a control schematic diagram of the self-adjusting deadweight balance position of fig. 2.
FIG. 5 is a schematic diagram of an autonomous regulated deadweight position control based on an improved simple particle-optimized PID controller.
Detailed Description
The invention will be further explained with reference to the drawings.
As shown in fig. 1, the self-adjusting self-weight balancing mechanism is composed of a balancing rod 1, a balancing block 2, a steel wire rope 3, a connecting piece 4, a driving small wheel 5, a speed-reducing driving wheel 6, a speed-reducing driven wheel 7, a direct current motor 8 and a photoelectric encoder 9. The balance weight 2 is arranged on the balance bar 1, the contact between the balance weight and the balance bar 1 is very smooth, the friction is very small, and the balance weight 2 can move along two directions of the balance bar 2. The balance bar 1 is fixed on the force feedback device through a connecting piece 4. The balance block driving deceleration driving wheel 6 is arranged on the direct current motor 8 and rotates coaxially with the direct current motor 8, the balance block driving deceleration driving wheel 6 drives the balance block driving deceleration driven wheel 7 to move through a steel wire rope, and the balance block driving deceleration driven wheel 7 drives the driving small wheel 5 to rotate coaxially with the driving small wheel.
For convenience of description, the self-adjusting self-weight balance mechanism of the force feedback device in fig. 1 is simplified into a dynamics analysis schematic diagram of the self-adjusting self-weight balance mechanism in fig. 2.
As shown in FIG. 2, the weight 2 has a mass mbThe gravity is GbThe acting force of the steel wire rope 3 on the balance block is Fp, and the friction force isFf3Balance weight 2 on balance bar 1 distance o2Instantaneous position is lbInstantaneous velocity ofInstantaneous accelerationThe balance weight 2 is upward Qo on the balance bar 12And o2The kinetic equation for sliding in the Q direction is: :
radius r of speed-reducing driving wheel 61Radius r of deceleration driven wheel 72Is 1/N, the rotating radius of the driving small wheel 5 is r3. The speed reduction driving wheel 6 is embedded on the rotating shaft of the direct current motor 8, rotates coaxially with the rotating shaft of the direct current motor 8 as a whole, and the rotating angles of the speed reduction driving wheel and the rotating shaft are the same and are marked as thetaARC1. The rotation angle of the driving small wheel 5 and the deceleration driven wheel 7 also rotate coaxially, and are the same as each other as a whole, and the rotation angle is represented by θARC2. Angle of rotation thetaARC1And thetaARC2The mathematical relationship between the two is as follows:
the small driving wheel 5 pulls the balance block 2 to move on the balance bar 1 through the steel wire rope 3, and the position of the balance block 2 on the balance bar is marked as lb8 degree of rotation theta of DC motorARC1And the angle of rotation theta of the driving small wheel 5ARC2The mathematical relationship of (1) is as follows:
the direct current motor 8 and the speed reduction driving wheel 6 are integrated, and the moment of inertia is J1. The deceleration driven wheel 7 and the driving small wheel 5 rotate coaxially and can also be regarded as a whole, and the moment of inertia of the deceleration driven wheel is J2。
The stress analysis is carried out on the deceleration driven wheel 7 and the driving small wheel 5 as a whole to obtain the deceleration driven wheel
Wherein, tau2The direct current motor 8 and the deceleration driving wheel 6 act on the whole of the deceleration driven wheel 7 and the driving small wheel 5, B2The viscous friction coefficient of the whole body of the small driving wheel 5 and the decelerating driven wheel 7 is shown, and the acting force of the balance block 2 on the small driving wheel 5 and the decelerating driven wheel 7 through the steel wire rope 3 iscFp, the force and Fp are mutually acting force and reacting force.
As shown in fig. 3, the dc motor 8 and the reduction drive pulley 6 are analyzed as a whole, and the relationship between the armature control of the dc motor by kirchhoff's theorem is as follows:
wherein, Um，Im，LmAnd RmRespectively representing the voltage, current, inductance and resistance, K, in the armature circuit of the DC motor 8eReferred to as the potential constant of the dc motor 8.
Output torque tau of direct current motor 81And armature loop current ImThe relationship is as follows:
τ1＝K1Im(7)
wherein, K1Is the torque constant of the dc motor 8.
Considering the mechanical characteristics of the direct current motor 8, according to newton's law, the torque equation of the direct current motor 8 can be solved:
wherein, J1And B1Representing the moment of inertia and the viscous friction coefficient of the rotor of the direct current motor 8, respectively. The output torque τ of the DC motor 8 can be obtained by substituting the expressions (1) to (5) into the expression (8)1And motor rotation angle thetaARC1Differential equation between:
wherein J represents the total rotational 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, τdThe torque generated by the gravity and the friction force applied to the balance weight of the self-adjusting weight balance mechanism (or the torque required to counteract the gravity and the friction force applied to the balance weight 2 during movement) is respectively expressed as follows:
by controlling the output torque tau of the DC motor 81The required motor rotation angle theta can be obtainedARC1Then l can be obtained according to the formulas (3) and (4)b＝r3θARC1N, so that the position l of the balancing mass 2 on the balancing bar 1 can be controlledb。
Laplace transform of equations (7), (8) and (9) yields (10), (11) and (12):
Um(S)＝Im(S)Rm+SLmIm(s)+SKeθARC1(S) (10)
wherein, Um(S)，Im(S) and θARC1(S) respectively represent Um，ImAnd thetaARC1Is performed by the laplace transform.
τ1(S)＝K1Im(S) (11)
Wherein, tau1(S) represents τ1The Ralsberg transform of (1).
τ1(S)＝S2JθARC1(S)+SBθARC1(S)+τd(S) (12)
Wherein: tau isd(S) represents τdThe Ralsberg transform of (1).
And then substituting the formulae (11) and (12) into the formula (10) to obtain:
when setting taudWhen (S) — 0, at this time,by rearranging equation (13), the following transfer function of the control system for autonomously adjusting the dead weight balance mechanism can be obtained:
when torque taudWhen (S) ≠ 0, the DC motor 8 generates a moment tau overcoming the gravity and friction force borne by the balance block 2d(S) required Voltage value of U'm(S) thus establishing U'm(S) and τdThe input/output equation of (S) is:
by combining the equations (14) and (15) and adopting the linear superposition principle, an overall control system of the self-main adjusting self-weight balancing mechanism can be constructed as shown in fig. 4.dθARC1The desired rotation angle of the DC motor 8 is determined according to the formula (4) to obtain the desired position of the balance weightConversion into the desired joint angledθARC1. Thus, the actual rotation angle θ of the motor 8ARC1And desired turning angledθARC1As input to the controller, Um(S) as the output of the controller for providing the input voltage to the armature of the DC motor 8 for driving the DC motor continuously to a desired rotation angledθARC1Close. Thus, the expected rotation angle of the DC motor 8 is continuously adjusted by the controllerdθARC1Angle of rotation theta with respect to the actualARC1Thereby continuously varying the armature input voltage Um(S) making the actual rotation angle thetaARC1Gradually reaching the desired rotation angledθARC1. Finally, the direct current motor 8 drives the speed reduction driving wheel 6, the speed reduction driving wheel 6 drives the speed reduction driven wheel 7 through the steel wire rope, the speed reduction driven wheel 7 drives the driving small wheel 5 to coaxially rotate, the driving small wheel 5 drives the steel wire rope 3, and the steel wire rope 3 drives the balance block 2 to reach the expected position of the balance rod 1
In fig. 4, the dc servo control system for autonomously adjusting the weight balance mechanism is a linear system, and a PID (proportional-derivative-integral) controller is used for position control. The lagrange transform of the PID controller is:
wherein, KpIs the proportional gain, KiIs the integral gain, KdIs the differential gain.
In order to select reasonable PID controller parameters, an improved simple particle optimization PID controller is invented, an improved simple particle algorithm is used, PID parameters are intelligently adjusted, and the principle is shown in FIG. 5. Selecting ITAE (Intergral of Time multiplex by Absolute error) index as the measure of proportional gain K of PID controllerpIntegral gain KiDifferential gain KdWhether the optimal evaluation index is reached is defined as follows:
where e (t) represents the error between the output and the input in the control system, and t is a time variable.
The specific optimization and adjustment steps are as follows:
(1) in the improved simple particle optimization algorithm, Y ═ Y (Y)1,Y2,…Yn) Means that n particles constitute one particle group; wherein Y represents a set of particle groups, Yi(i ═ 1,2, …, n) represents the position of the ith particle in the population; each particle flight position search space of the particle swarm is a 3-dimensional vector space, and each 1-dimensional in the three-dimensional position search space respectively represents the proportional gain K of the PID controllerpIntegral gain KiAnd a differential gain Kd(ii) a Assuming that the total number of particle iterations is N, then the position of the ith particle in the kth iteration is represented as:wherein,andproportional gain, integral gain and differential gain of the ith particle in the kth iteration are respectively obtained; these three parameters are random numbers and their search intervals range within an interval with a minimum value of 0 and a maximum value of p (p > 0).
(2)Is the position of each particle i in the kth iteration of the particle swarm, respectivelyAndsubstituting into FIG. 5 as the corresponding proportional gain, integral gain, differential gain in the PID controllerd(S) anddθARC1(S) as an input excitation signal, and in the kth iteration, its corresponding ITAE indicator is calculated as a fitness value for the improved simple particle optimization PID algorithm using equation (17).
(3) According to all fitness values ITAE of all n particles in the particle swarm after the evolution iteration from the 1 st time to the k-th time, finding out the global optimal fitness value as follows:wherein,andindividual watchAnd (3) showing the global optimal proportional gain, the global optimal integral gain and the global optimal differential gain of all n particles in the particle swarm in k iterations.
(4)Yi(k+1)＝λ1η1Yi(k)+λ2η2[Pg(k)-Yi(k)]As an evolutionary iterative update equation, the ith particle in all the n particles is updated in the (k + 1) th iteration, wherein ηi(i ═ 1,2) respectively represent random numbers between 0 and 1, λ1And λ2Each representing a cognitive learning rate and a social learning rate, both of which are generally constant values 1.49445. .
(5) And boundary condition agreement: if the particle group is composed of n particles, after the iterative computation of the k-th iteration, the ith particle position Xi(k) Is out of the specified interval: the minimum value of this interval is 0 and the maximum value is ρ (ρ > 0). Then the ith particle Xi(k) A certain dimension of the position must be reassigned to 0 or ρ (ρ > 0). If the dimension position variable is less than 0, the dimension position variable is designated as 0; the dimension position variable is greater than ρ (ρ > 0), then the dimension position variable is designated as ρ (ρ > 0).
(6) If the evolution iteration number reaches the maximum iteration number N, the iteration is ended, and the global optimal proportional gain, the global optimal integral gain and the global optimal differential gain after all the evolution iterations are found. Otherwise, returning to the step (2) to continue the sequential execution. Thus, the proportional gain, integral gain and differential gain of the optimal PID control of the system can be found according to the improved simple particle optimization PID control algorithm in FIG. 5.
In order to overcome the high-frequency interference introduced by differential signals in PID, Laplace transform in a PID controllerThe first-order low-pass filter is connected in series with the differential term in the filter to formThe smoothing system outputs a vibration in response, wherein 0 < xi < 1.
Claims (1)
1. A control algorithm for an automatic self-adjusting self-weight balance mechanism of force feedback equipment is characterized by comprising the following steps:
step 1: torque tau generated by gravity and friction of balance block of self-regulating self-weight balance mechanism when force feedback equipmentdWhen (S) is 0, the motor armature voltage is establishedFor input, the motor rotation angle theta is usedARC1Control system for self-regulating dead weight balancing mechanism for outputA transfer function; when the torque tau generated by the gravity and the friction force borne by the balance block of the self-adjusting self-weight balance mechanismdWhen (S) ≠ 0, establishing that a direct current motor overcomes taudVoltage U 'required for (S) moment'm(S) as input, using motor output torque taud(S) is the control system transfer function of the output self-adjusting self-weight balance mechanism;
step 2: adopting the linear superposition principle to establish the integral control system function of the self-adjusting self-weight balance mechanism of the force feedback interactive equipment,dθARC1is the desired rotation angle of the DC motor, and establishes the desired position of the balance weight on the balance barAngle of rotation desired for DC motordθARC1As a function of the actual rotational angle theta of the motorARC1And desired turning angledθARC1The error is used as the input of the integral control system of the self-adjusting self-weight balance mechanism, and is used as the input voltage U of the direct current motor armature after being adjusted by the improved simple particle optimization PID control algorithmm(S) wherein, in the above step,U′m(S) is a torque for generating a torque tau for counteracting the gravity and the friction of the balance weight of the self-adjusting weight balance mechanismd(S) a desired voltage of the voltage,is used for driving the direct current motor to rotate to a desired rotation angledθARC1The required voltage; the actual rotation angle theta of the direct current motor is continuously adjusted by the controllerARC1And desired turning angledθARC1By continuously varying the armature input voltage Um(S) so that the actual rotation angle thetaARC1Gradually reaching the desired rotation angledθARC1(ii) a Finally, a speed reduction driving wheel of the direct current motor drives a speed reduction driven wheel, the speed reduction driven wheel drives a driving small wheel, the driving small wheel drives a balance block, and balance is achievedThe rod reaches the desired position
And step 3: designing and improving a simple particle optimization PID control algorithm for controlling the expected position of a balance block of an automatic self-adjusting self-weight balance mechanism on a balance rodSelecting ITAE index as the proportional gain K for measuring and improving PID controller in simple particle optimization PID control algorithmpIntegral gain KiAnd a differential gain KdWhether the optimal index is reached or not; in the improved simple particle optimization PID control algorithm, the improved simple particle optimization algorithm is used for optimizing and controlling the proportional gain K of the PID controllerpIntegral gain KiAnd a differential gain KdThe method comprises the following specific steps:
(1) in the improved simple particle optimization algorithm, Y ═ Y (Y)1,Y2,…Yn) Means that n particles constitute one particle group; wherein Y represents a set of particle groups, Yi(i ═ 1,2, …, n) represents the position of the ith particle in the population; each particle flight position search space of the particle swarm is a three-dimensional vector space, and each dimension in the three-dimensional position search space respectively represents the proportional gain K of the PID controllerpIntegral gain KiAnd a differential gain Kd(ii) a Assuming that the total number of particle iterations is N, then the position of the ith particle in the kth iteration is represented as:wherein,andproportional gain, integral gain and differential gain of the ith particle in the kth iteration are respectively obtained; the three parameters areRandom numbers, their search ranges are all within the interval with the minimum value of 0 and the maximum value of rho, wherein rho>0；
(2)Is the position of particle i in the kth iteration of the particle swarm, respectivelyAndas proportional gain, integral gain, differential gain in the PID controller, respectivelyd(S) anddθARC1as input excitation signals, in the k iteration, the corresponding ITAE indexes are used as fitness values for improving the simple particle optimization algorithm;
(3) according to all fitness values ITAE of all n particles in the particle swarm from the 1 st evolutionary iteration to the kth evolutionary iteration, the global optimal fitness value can be found as follows:wherein,andrespectively representing the global optimal proportional gain, the global optimal integral gain and the global optimal differential gain of all n particles in the particle swarm in k iterations;
(4)Yi(k+1)＝λ1η1Yi(k)+λ2η2[Pg(k)-Yi(k)]as an evolutionary iterative update equation, the ith particle in all the n particles is updated in the (k + 1) th iteration, wherein ηi(i ═ 1,2) respectively represent random numbers between 0 and 1, λ1And λ2Each represents and recognizesLearning rate and social learning rate are known, and constant values 1.49445 are taken for the learning rate and the social learning rate;
(5) and boundary condition agreement: after the particle swarm composed of n particles is subjected to the kth iterative computation, the position Y of the ith particle isi(k) Is out of the specified interval: the minimum value of the interval is 0 and the maximum value is rho, wherein rho>0; then the ith particle Yi(k) A certain dimension of the position must be reassigned to 0 or ρ; if the dimension position variable is less than 0, the dimension position variable is designated as 0; if the dimension position variable is greater than ρ, the dimension position variable is designated as ρ;
(6) if the operation reaches the maximum iteration number N, ending the iteration, and finding out the global optimal proportional gain, the global optimal integral gain and the global optimal differential gain after all evolutionary iterations; otherwise, returning to the step (2) to continue the sequential execution; finally, finding out the proportional gain, integral gain and differential gain of the optimal PID control;
and 4, step 4: in order to overcome the high-frequency interference introduced by differential signals in the PID controller, Laplace transform in the PID controllerThe first-order low-pass filter is connected in series with the differential term in the middle to formAnd smoothing the oscillations of the system output response, wherein 0 < xi < 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610028359.9A CN105573121B (en) | 2016-01-18 | 2016-01-18 | A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610028359.9A CN105573121B (en) | 2016-01-18 | 2016-01-18 | A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105573121A CN105573121A (en) | 2016-05-11 |
| CN105573121B true CN105573121B (en) | 2018-07-20 |
Family
ID=55883387
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610028359.9A Active CN105573121B (en) | 2016-01-18 | 2016-01-18 | A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105573121B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106970527B (en) * | 2017-03-30 | 2020-04-10 | 奥克斯空调股份有限公司 | PID control engineering implementation method |
| CN109584324B (en) * | 2018-10-24 | 2022-12-06 | 南昌大学 | Positron Emission Tomography (PET) reconstruction method based on automatic encoder network |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101623864A (en) * | 2009-08-13 | 2010-01-13 | 天津大学 | Force feedback type master manipulator with deadweight balance property |
| CN102320040A (en) * | 2011-08-11 | 2012-01-18 | 南昌大学 | Force feedback interactive device for automatically regulating balance of dead weight |
| CN103406908A (en) * | 2013-07-31 | 2013-11-27 | 北京邮电大学 | Force feedback hand controller with three degrees of freedom rotation |
| US8994643B2 (en) * | 2003-10-30 | 2015-03-31 | 3D Systems, Inc. | Force reflecting haptic interface |
-
2016
- 2016-01-18 CN CN201610028359.9A patent/CN105573121B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8994643B2 (en) * | 2003-10-30 | 2015-03-31 | 3D Systems, Inc. | Force reflecting haptic interface |
| CN101623864A (en) * | 2009-08-13 | 2010-01-13 | 天津大学 | Force feedback type master manipulator with deadweight balance property |
| CN102320040A (en) * | 2011-08-11 | 2012-01-18 | 南昌大学 | Force feedback interactive device for automatically regulating balance of dead weight |
| CN103406908A (en) * | 2013-07-31 | 2013-11-27 | 北京邮电大学 | Force feedback hand controller with three degrees of freedom rotation |
Non-Patent Citations (2)
| Title |
|---|
| Design and Implementation of a haptic interface device with auto-regulation gravity;李春泉、刘小平、徐少平;《IEEE International Workshop on Haptic Audio Visual Environments and Games》;20111231;158-163 * |
| PID Controller Tuning using a Modified Simple Particle Swarm Optimization Algorithm;李春泉，刘小平，徐少平;《International Journal of Advancements in Computing Technology》;20120930;第4卷(第16期);1-8 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105573121A (en) | 2016-05-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Keshmiri et al. | Modeling and control of ball and beam system using model based and non-model based control approaches | |
| Jia et al. | An improved particle swarm optimization (PSO) optimized integral separation PID and its application on central position control system | |
| CN110552961B (en) | Active magnetic bearing control method based on fractional order model | |
| CN108984815B (en) | Control method of vehicle-mounted flywheel battery radial suspension supporting system based on working condition | |
| CN111103798B (en) | AGV path tracking method based on inversion sliding mode control | |
| US20220004160A1 (en) | Positioning control device and positioning method | |
| JP4918682B2 (en) | Ultrasonic motor control method, ultrasonic motor control apparatus, and program for controlling ultrasonic motor | |
| Zhang et al. | New PID parameter autotuning for nonlinear systems based on a modified monkey–multiagent DRL algorithm | |
| CN110875703B (en) | Machine learning device, control system, and machine learning method | |
| CN105573121B (en) | A kind of autonomous adjusting dead weight balance mechanism control algolithm of force feedback equipment | |
| CN113703319A (en) | Joint module inequality constraint optimal robust control method based on reinforcement learning | |
| Huynh et al. | Comparative application of model predictive control strategies to a wheeled mobile robot | |
| Mohammed et al. | Elevation, pitch and travel axis stabilization of 3DOF helicopter with hybrid control system by GA-LQR based PID controller | |
| Salem | Modeling, Simulation and Control Issues for a Robot ARM; Education and Research (III) | |
| CN109039190A (en) | A kind of four-degree-of-freedom magnetic suspension motor suspension system decoupling control method | |
| KR101918101B1 (en) | Method for designing the controller for suppressing the maximum amplitude of trajectory tracking errors and Controller using the same | |
| Abdulla et al. | Roll control system design using auto tuning LQR technique | |
| Majczak et al. | Comparison of two efficient control strategies for two-wheeled balancing robot | |
| WO2023165177A1 (en) | Method for constructing controller of robot, motion control method for robot and apparatuses, and robot | |
| Xu | A Comparative Study of PID and LQR Control Strategies Applied to Inverted Pendulum Systems | |
| Shamseldin | Real-time inverse dynamic deep neural network tracking control for Delta robot based on a COVID-19 optimization | |
| CN111505935B (en) | Automatic guided vehicle control method and system | |
| Auzan et al. | Path Tracking and Position Control of Nonholonomic Differential Drive Wheeled Mobile Robot | |
| Neunert et al. | Adaptive real-time nonlinear model predictive motion control | |
| Agrawal | Optimal Controller Design for Twin Rotor MIMO System |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |