JP3236607B2 - Digital servo controller - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial applications]

The present invention relates to a digital servo control device capable of correcting a dynamic backlash caused by a tracking delay during driving of a mechanical system.

[Prior art]

Conventionally, as backlash correction to compensate for mechanical gaps that occur when the direction of rotation of the gears or the gap between the tooth surfaces when the gears mesh with each other is reversed, when the command direction is reversed, this is equivalent to the mechanical gap of the mechanical system. The amount to be corrected is added to the command value.

[Problems to be solved by the invention]

By the way, as the backlash when the moving direction of the mechanical system is reversed at high speed, there is a dynamic backlash caused by a following delay of the servo system in addition to the static backlash due to the play of the conventional mechanical system. This dynamic backlash causes a position deviation between the target value and the actual position of the feed axis for a while even if the target value of the commanded position is reversed, and the motor moves in the same direction due to the position deviation. This occurs because the speed control for rotating the motor is continued. This dynamic backlash causes the finished shape of the workpiece to be deformed. SUMMARY OF THE INVENTION The present invention has been made to solve the above-described problems, and an object of the present invention is to correct dynamic backlash caused by a delay in following a servo system and improve the accuracy of a machining shape of a workpiece. That is.

[Means for Solving the Problems]

A configuration of the invention for solving the above-mentioned problem has a position feedback loop, a speed feedback loop, and a current feedback loop, and in a digital servo control device that gives a command value and a feedback value in digital amounts, Speed calculation means for calculating a commanded speed from time variation, and acceleration calculation means for calculating a commanded acceleration from a time variation of the commanded speed calculated by the speed calculation means,
When the sign of the command speed calculated by the speed calculating means changes and the magnitude of the command acceleration calculated by the acceleration calculating means is larger than a predetermined value, dynamic backlash correction, which is a tracking delay during driving of the mechanical system, is required. And a backlash correction unit that corrects the speed deviation of the speed feedback loop with a correction amount according to the command acceleration when the correction determination unit determines that dynamic backlash correction is necessary. It is characterized by having.

[Action]

The command speed is calculated from the time variation of the target position commanded by the speed calculating means. Also, the command acceleration is calculated from the time variation of the command speed calculated by the speed calculating means by the acceleration calculating means. When the sign of the command speed calculated by the speed calculating means is inverted by the correction determining means, and the magnitude of the command acceleration calculated by the acceleration calculating means is larger than a predetermined value, It is determined that dynamic backlash correction, which is a delay, is necessary. When the backlash correction means determines that dynamic backlash correction is required by the correction determination means, the speed deviation of the speed feedback loop is corrected by a correction amount having a magnitude corresponding to the commanded acceleration. That is, at the time of reversing the moving direction of the position, the target position is temporarily reduced, and the target position is set to zero or the target position in the opposite direction.
It is possible to temporarily apply an acceleration in the opposite direction to the servomotor to improve the responsiveness of reversing the actual position of the feed axis.

【Example】

Hereinafter, the present invention will be described based on specific examples. FIG. 1 is a block diagram showing the configuration of a digital servo control device according to the present invention. Digital servo controller 10 is mainly composed of CPU 11, ROM
12, RAM13, digital signal processor (hereinafter "DS
P) 14, a common RAM 17, A / D converters 15a and 15b, and a current value counter 16. A keyboard 21 and a CRT display device 22 are connected to the CPU 11 via an interface 19. The output of the DSP 14 is input to the inverter 25, and the inverter 25 drives the servo motor 31 according to the output signal of the DSP 14. A synchronous motor is used as the servo motor 31, and the load current of the servo motor 31 is controlled by the PWM voltage control of the inverter 25, and as a result, the output torque is controlled. The u-phase and v-phase load currents of the servo motor 31 are CT32a, 32
b, and amplified by the amplifiers 18a and 18b. Outputs of the amplifiers 18a and 18b are input to A / D converters 15a and 15b, sampled at a predetermined cycle, and converted into digital values. The sampled value is input to the DSP 14 as a feedback value of the instantaneous load current. Further, a pulse encoder 33 is connected to the servo motor 31, and its current position is detected. The output of the pulse encoder 33 is connected to the current value counter 16 via the waveform shaping / direction discriminating circuit 34. Current value counter 16 via waveform shaping / direction determination circuit 34
The output signal from the pulse encoder 33, which is input to the controller, increases or decreases the value of the current value counter 16. The value of the current value counter 16 is read by the DSP 14 as a current position feedback value, and is compared with the target value output from the CPU 11 by the DSP 14 to calculate a position deviation. And with DSP14,
A speed target value is calculated based on the position deviation. Further, the current position feedback value input to the DSP 14 is differentiated, and a speed feedback value is calculated. DSP14
Thus, the speed target value determined according to the position deviation is compared with the speed feedback value to calculate the speed deviation, and the current target value is calculated based on the speed deviation. With the DSP 14, this current target value is adjusted by the amplifiers 18a, 18b and
The current deviation is calculated by comparing with the current feedback value of the load current detected by the CTs 32a and 32b via the A / D converters 15a and 15b. The instantaneous current command value at that time is calculated based on the instantaneous current deviation at that time, the accumulated value of the instantaneous current deviation, and the current target value, that is, by a proportional integral calculation. The instantaneous current command value is compared with the high-frequency triangular wave,
Voltage control to control on / off of 25 phase transistors
A PWM signal is generated. The voltage control PWM signal is output to the inverter 25, and the transistors of each phase of the inverter 25 are driven. By the switching of the inverter 25, the load current of each phase is controlled to the target current value. The positioning of the servo motor 31 is completed when the CPU 11 determines that the output value of the current value counter 16 has become equal to the target value of the position. Also, u sampled by the A / D converters 15a and 15b
The DSP 14 converts the phase and v-phase load current values into dq. As described above, the digital servo control device of the present embodiment is configured by three feedback loops of position, speed, and current. The lower feedback loop requires higher responsiveness. For example, the lowest current feedback loop is 100 μs, the speed feedback loop is several times as long, and the position feedback loop is synchronized at several times more time. Data sampling is performed, and the processing of each feedback loop is performed. Next, the operation of the device of this embodiment will be described. The program shown in FIG. 2 is repeatedly executed by the DSP 14 every predetermined minimum cycle. In step 100, it is determined whether or not the current execution cycle is the position deviation calculation timing, and if the position deviation calculation timing, the process proceeds to step 102, where the target position PO commanded by the CPU 11 is input from the common RAM. Next, in step 104 for achieving the speed calculating means, the current command speed VO is calculated from the target position of the present input, the target position of the previous input, and the elapsed time therebetween. Next, in step 106 for achieving the acceleration calculation means, the current command acceleration AO is calculated from the current command speed and the previous command speed. Next, the process proceeds to step 108 for achieving the correction determining means, and it is determined whether the backlash correction condition is satisfied. That is, the sign of the command speed VO calculated in step 104 is inverted, and it is determined whether the magnitude of the absolute value of the command acceleration AO calculated in step 106 is equal to or greater than a predetermined threshold At.
The inversion of the sign of the command speed VO reverses the commanded movement direction,
When the absolute value of the command acceleration AO is large, it means that the amount of the tracking delay is large. If the correction condition is satisfied, the determination at Step 108 is YES, and the routine goes to Step 110. In step 110, the backlash correction amount B is equal to the command acceleration A.
It is calculated by a value proportional to O, that is, by the following equation. B = k · AO (k: proportionality constant) If the backlash correction condition is not satisfied in step 108, the process proceeds to step 112, where the backlash correction amount B is set to zero. Then, the process proceeds to step 114, and the target position for management in the DSP 14 is corrected by the following equation. PO = PO + B That is, the direction of the management target position is shifted by the backlash correction amount with respect to the commanded target position. Next, step 116 for achieving the backlash correction means
Then, the current value of the position held in the current value counter 16, that is, the current position is read, and the position deviation from the corrected target position for management is calculated. Next, in step 118, a speed target value is calculated according to the position deviation. Here, in step 114,
If the backlash correction of the target position is performed, it means that the backlash correction has been performed also on the speed target value. This position feedback control is executed at the timing indicated by the signal S1 in FIG. Next, in step 120, it is determined whether or not the current execution cycle is a timing deviation calculation timing. If the speed deviation is calculated, the process proceeds to step 122, where the current value (electrical angle) θ (n) of the position held in the current value counter 16 is read. Next, in step 124, the current value (electrical angle) θ of the position read at the time of the previous speed deviation calculation timing
From (n-1) and the speed control cycle D, the current value V (n) of the speed in the current speed control period is calculated by the following equation. v (n) = (θ (n) −θ (n−1)) / D (1) Further, a deviation from the speed target value set in step 118, that is, a speed deviation is calculated. Here, if the speed target value in step 118 has the backlash correction, it means that the speed deviation has also been backlash corrected. That is, a reversing force acts on the servo motor 31 to apply a brake to reverse rotation. Then, in the next step 126, current target values of the d-axis component and the q-axis component are calculated according to the speed deviation. This speed feedback control is executed at the timing indicated by the signal S2 in FIG. Next, in step 128, it is determined whether or not the current execution cycle is the current deviation calculation timing. If it is the current deviation calculation timing, the process proceeds to step 130. Step 130 and subsequent steps are current feedback control,
This control is executed at the timing indicated by the signal S3 in FIG. In step 130, using the current detection time Delta] t ₁ at the next step 132, measured from the beginning of the current control period, the control time Delta] t ₂ of the load current measured from the beginning of the current control period, corresponding to the time the electric current detecting when the electrical angle theta ₁ to the control when the electrical angle theta ₂ is the angular is interpolation. θ ₁ = θ (n) + ΔV (n) · Δt ₁ (2) θ ₂ = θ (n) + ΔV (n) · Δt ₂ (3) The times Δt ₁ and Δt ₂ and the electrical angles θ ₁ and θ ₂ corresponds as shown in FIG. Next, the routine proceeds to step 132, where the current values Iu and Iv of the u-phase and v-phase instantaneous load currents are read from the A / D converters 15a and 15b. Next, in step 134, the current values Iu, Iv
Is subjected to dq conversion, and a d-axis component Id and a q-axis component Iq are calculated by the following equation. As is well known, the dq coordinate system is a coordinate system in which the d-axis has the same phase as the exciting magnetic field and the q-axis has a phase difference of 90 ° in electrical angle from the exciting magnetic field. The d-axis component is q
The axis component represents the active ingredient. Next, in step 136, the current command values Id ^* , d and d of the d-axis component and the q-axis component at the present time are calculated by proportional integration based on the d-axis component and the q-axis component current target values set in the step 126. Iq ^* is calculated. Next, in step 138, the current command values Id ^* , Iq ^* are subjected to inverse dq conversion according to the following equation, and the current command values Iu ^* , I
v ^* and Iw ^* are calculated. Note that Iw ^* is calculated by Iw ^* = − (Iu ^* + Iv ^* ). Next, in steps 140 and 142, each phase current command value Iu
A series of PWM signals in one control cycle are generated using the level relationship between the high-frequency triangular waves of ^* , Iv ^* , and Iw ^* , that is, using the average voltage method. As shown in FIG. 5, a series of PWM signals can be represented by a voltage vector indicating a voltage application state of each phase.
The rotating magnetic field vector is represented as the integral of this voltage vector. Therefore, as shown in FIG. 6, the locus of the tip of the rotating magnetic field vector is drawn by the sum of each voltage vector × duration. In order to position the rotating magnetic field at a point on the circumference at an angle of 2π / n by the shortest path, the three adjacent voltage vectors and the zero vector V ₀ shown in FIG. The inverter 25 needs to be controlled by two vectors. The combination of these three voltage vectors uniquely corresponds to the phase of the rotating magnetic field. The correspondence table of the combination of the phase of the rotating magnetic field and the voltage vector (since the zero vector V ₀ is always one element of the combination, only the combination of the two voltage vectors is sufficient)
As shown in FIG. 7, it is stored in the ROM 12 in advance. In step 140, the above table in the ROM 12 is searched from the control electrical angle θ ₂ (phase of the rotating magnetic field) to determine the combination of the voltage vectors at that time. In step 142, the duration of each voltage vector t _1, t _2, t
₃ is calculated. For example, as shown in FIG. 8, the combination of the voltage vectors is V ₆ = (1,1,0), V ₁ = (1,0,0), V ₀ = (0,0.
0), the duration t ₁ , t ₂ , t of each voltage vector
₃ is calculated. In this embodiment, a well-known average voltage method is used as the calculation method. That is, the phase current command values Iu ^*, Iv ^*, among Iw ^*, I ₁ ^* two large absolute value in descending ^order, when the I ₂ ^*, duration t _1, t _2, t ₃ the following It is obtained by the formula. ^{_{t 1 = | 2I 2 * +}} I 1 * | · T / Vdc ... (8) t 2 = | I 1 * -I 2 * | · T / Vdc ... (9) t 3 = T- (t 1 + t 2) (10) where T is the cycle and Vdc is the applied DC voltage. Next, at step 144, PWM signal according to a set of voltage vectors are output by the duration _{t 1, t 2, t 3} . For example, as shown in FIG. 8, V ₆ = (1,1,0), V ₁ = (1,
(0,0), V ₀ = (0,0,0) in the order of durations t ₁ , t ₂ , t ₃ . In other words, in the U phase, a voltage is applied by t ₁ + t ₂ , in the V phase, a voltage is applied by t ₁ , and in the W phase, no voltage is applied during the control period. Thus, the processing of one execution cycle is completed. The run cycle, the minimum being executed in the control period of the current feedback loop is controlled by an integral multiple n _1, the velocity feedback loop an integral multiple n ₂ is controlled, the position feedback loop in its integer multiples n ₃ Are controlled in steps 100, 120, and 128, the number of times is set as a reference for determination. Here, n ₁ <n ₂ ≦ n ₃ . By repeating the above cycle, the position, speed, and current feedback control is performed at the timing shown in FIG. However, the timing shown in FIG. 4 is detected by measuring the time from when the CPU 11 executes the program. As described above, when the position changes direction, the speed deviation is corrected in the opposite direction, so that the servomotor 31 generates a rotational force in the opposite direction, which acts as a brake or acceleration in the opposite direction. As a result, the backlash caused by the position tracking delay is corrected. Next, the meaning of the backlash correction according to the present invention will be described. The command speed degree VO is calculated from the time change of the commanded target position as shown in FIG. 3 (a), and the command acceleration AO is calculated from the time change of the command speed VO as shown in FIG. 3 (b). Then, the sign of the command speed VO is reversed, that is, the moving direction is reversed,
In addition, when the command acceleration AO fluctuates greatly, there is a delay in following the actual rotation angle of the servo motor 31 without backlash correction. That is, although the sign of the command speed VO is inverted, the servo motor 31 continues to rotate in the same direction at a rotation speed proportional to the following deviation. Here, when the sign of the command speed VO is reversed, a predetermined threshold value At is set for the command acceleration AO as shown in FIG. 3 (b). If the command acceleration AO is equal to or greater than the threshold At, the threshold
A correction amount B (see FIG. 3 (c)) corresponding to the magnitude exceeding At is obtained. As shown in FIG. 3 (d), when the sign of the commanded target position PO is inverted, the backlash correction is made by the backlash correction amount B proportional to the command acceleration at that time. As a result, this is equivalent to the commanded target position being corrected in the direction opposite to the current rotation direction. Further, the speed deviation, that is, the difference between the speed target value and the current speed value is also determined by the backlash correction amount B in the direction opposite to the current rotation direction.
Is corrected. As is clear from the above, in the above embodiment,
Although the management target position PO is corrected, the speed deviation may be corrected as a result. Therefore, in addition to the above-described correction, even if the current value of the management position, the target value of the speed, and the current value of the speed are corrected, the speed deviation is similarly corrected.

【The invention's effect】

The present invention determines that the dynamic backlash correction is necessary when the sign of the commanded speed is inverted and the magnitude of the commanded acceleration is larger than a predetermined value. The speed deviation is corrected by a correction amount corresponding to the command acceleration. Therefore, when the feed direction is changed, the speed deviation is corrected in the decreasing direction, so that the dynamic backlash due to the following delay is corrected, and the finished shape of the workpiece is improved.

[Brief description of the drawings]

FIG. 1 is a block diagram showing the configuration of a digital servo control device according to a specific embodiment of the present invention. FIG. 2 is a flowchart showing a processing procedure by a DSP used in the apparatus of the embodiment. FIG. 3 is a timing chart showing a command speed, a command acceleration, a backlash correction amount, and a timing of a target position after the correction by the DSP.
FIG. 4 is a timing chart showing the timing of the position, speed, and current feedback control by the DSP. FIG. 5 is a vector diagram showing a voltage vector corresponding to a PWM signal. FIG. 6 is a vector diagram showing a relationship between a voltage vector and a rotating magnetic field. FIG. 7 is an explanatory diagram showing a correspondence relationship between a set of a phase of a rotating magnetic field and a voltage vector. FIG. 8 is a timing chart showing the PWM signal. 10 Digital servo controller 11 CPU, 12 ROM, 13 RAM 14 DSP (digital signal processor) 15a, 15b A / D converter 16 Current counter 25 Inverter , 31 ... servo motor 32a, 32b ... current transformer (CT) 33 ... pulse encoder

────────────────────────────────────────────────── ─── Continuation of the front page (56) References JP-A-2-93711 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) G05D 3/00-3/20

Claims (1)

(57) [Claims] 1. A digital servo control device having a position feedback loop, a speed feedback loop, and a current feedback loop, in which a command value and a feedback value are given in digital amounts, calculates a command speed from a time variation of a commanded target position. Speed calculation means for calculating, a command acceleration calculated from a time variation of the command speed calculated by the speed calculation means, a sign of the command speed calculated by the speed calculation means changes, and the acceleration calculation When the magnitude of the command acceleration calculated by the means is larger than a predetermined value, a correction determining means for determining that dynamic backlash correction which is a tracking delay during driving of the mechanical system is necessary; and a dynamic backlash by the correction determining means. When it is determined that correction is necessary, the speed feedback is performed with a correction amount corresponding to the command acceleration. A digital servo control device comprising: a backlash correction unit configured to correct a speed deviation of a loop.