JP3197898B2 - Servo motor control method - Google Patents

Description

Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control method of a servomotor for driving a feed shaft of a machine tool or an arm of a robot.

FIG. 4 is a block diagram of a position / speed control system for controlling a servomotor.

In FIG. 4, reference numeral 10 denotes a block for decomposing a movement command Mc distributed every predetermined distribution cycle from a control device such as a numerical control device for controlling a machine tool or a robot into a movement command Pc every position / velocity loop processing cycle. Reference numeral 11 denotes an integrator (error register) block for the position loop processing. The position feedback amount Pf for each position / speed loop processing cycle is calculated from the movement command Pc output from the block 10 for each position / speed loop processing cycle. The position deviation εp is obtained by integrating the reduced values. Reference numeral 12 denotes a position gain block for obtaining a speed command Vc by multiplying the position deviation εp by a position gain Kp. 13 is the speed command in the speed loop processing integrator block
The speed deviation εv obtained by subtracting the speed feedback amount Vf from Vc is integrated. 14 is a block of an integral constant in the speed loop, 15 is a block of a proportional constant of the speed loop, and subtracts a value obtained by multiplying the output Sv of the integrator 13 by the integral constant k1 by a value obtained by multiplying the speed feedback signal Vf by the proportional constant k2. To obtain the torque command Tc. Reference numeral 16 denotes a block representing a transfer function of a mechanical part of the servomotor, Kt represents a torque constant, and Jm represents inertia.

The movement command Mc commanded every distribution cycle is decomposed into the movement command Pc for each position / speed loop processing cycle, and from the movement command Pc,
A value obtained by subtracting the movement amount Pf of the servo motor for each position / speed cycle is integrated by an integrator 11 to obtain a position deviation εp.
Is subtracted from the actual speed Vf of the servo motor from the speed command Vc from the position loop process obtained by multiplying the speed gain by the position gain Kp to obtain a speed deviation εv. Then, the speed deviation εv is integrated by the integrator 13, and the value obtained by multiplying the integral value Sv by the integration constant k1 is subtracted by the value obtained by multiplying the actual speed Vf of the servo motor by the proportional constant k2 to obtain a torque command (current command). ) Find Tc. And
The servo motor is driven via an inverter or the like.

The above-described servo motor control method is well known in the art, and control of this servo motor by a servo circuit constituted by an analog circuit, and digital servo control by software using a pull processor are also conventionally known. .

When the servo circuit shown in FIG. 4 is performed by software using a processor, the processing of the integrator 13 in the speed loop is performed as follows.

The output of the speed loop integrator in the speed loop processing cycle n is Sv (n), the speed command in the same cycle n is Vc (n),
Assuming that the speed feedback amount in the same period n is Vf (n), the integrator 13 of the speed loop obtains the output Sv (n) by performing the calculation of the following first equation.

Sv (n) = Sv (n-1) + (Vc (n) -Vf (n)) (1) The operation shown in the above first expression is a process of complete integration. In some cases, complete integration is performed.

 Sv (n) = k3.Sv (n-1) + (Vc (n) -Vf (n)) (2) where k3 is a constant and 0 <k3 <1.

When configuring a servo circuit with an analog circuit, there is leakage current from the capacitor of the integrator in the speed loop.
In the case of an analog servo circuit, the incomplete integration represented by the above-described second equation is performed.

Problems to be Solved by the Invention When the integrator of the speed loop performs the complete integration represented by the first equation, in the steady state, the speed command Vc and the speed feedback amount Vf completely coincide with each other. Deviation ε
p becomes “0”. Also, the position deviation ε at a constant speed
p has a characteristic that it becomes constant irrespective of friction or disturbance torque and servo stiffness (disturbance suppression characteristic) increases.

However, even when the movement command Pc and the speed command Vc become “0” and the input of the integrator of the speed loop becomes “0”, a certain value is held in the integrator. It is output as a torque command Tc, and the magnitude of this torque command Tc is
If it is higher than the static friction level of the mechanical system driven by the servomotor, it cannot be stopped according to the command, and overshoot occurs.

FIG. 5 is an explanatory diagram for explaining that overshoot occurs when complete integration is used for the integrator of the speed loop.

For ease of explanation, it is assumed that “1” is output as the movement command Pc as shown in FIG. At first, the servomotor does not rotate due to mechanical friction, etc.
The position feedback signal Pf and the speed feedback signal Vf are not generated as shown in FIGS. Therefore, as shown in FIG. 5 (c), the position deviation .epsilon.p (= 1) is held in the integrator 11 of the position loop. This position deviation εp
Is input to the integrator 13 of the speed loop as the speed command Vc, and the output Sv of the integrator 13 sequentially increases as shown in FIG. Then, the torque command Tc generated from the output Sv of the integrator also increases sequentially, as shown in FIG.
When the torque command Tc exceeds the static friction of the mechanical system, the saw motor rotates, the position feedback Pf and the speed feedback Vf are output, and the position deviation εp becomes “0”.
Therefore, the output Sv of the integrator holds the integrated value at that time. On the other hand, the torque command Tc becomes a value reduced by multiplying the speed feedback amount Vf by the proportional constant k2 (see FIG. 5 (f)). However, when the reduced torque command Tc is larger than the dynamic friction of the mechanical system, the servomotor continues to rotate, the moving position overshoots the command position, and the speed feedback signal Vf is output (see FIG. b), (d)). As a result, the position deviation εp
Is a negative value, and the speed command Vc
Changes the output of the integrator of the speed loop to a negative value,
The reverse torque command Tc is output. And
When the negative torque command becomes equal to or larger than the static friction torque of the mechanical system, the servo motor rotates in the reverse direction, the rotational position of the servo motor becomes the command position, and the position deviation εp becomes “0”.

As described above, when the integrator of the speed loop is implemented by complete integration, an overshoot occurs when stopping.
On the other hand, if the incomplete integration as shown in the second equation is performed by the integrator of the speed loop, the problem of overshoot is reduced because the value of the output Sv of the integrator is reduced, but the speed command Vc And the speed feedback signal Vf are not equal, so that a positional deviation occurs during stoppage. Then, there arises a problem that the position deviation fluctuates due to a disturbance torque received by the servomotor during the movement at a constant speed, and the servo rigidity is deteriorated.

SUMMARY OF THE INVENTION It is an object of the present invention to provide a servomotor control method which has servo rigidity and reduces overshoot at the time of stop.

Means for Solving the Problems According to the present invention, in a servomotor control system in which a processor executes a position / velocity loop process, a movement command in a distribution cycle one or more ahead of a distribution cycle at that time is set to “0”. ), The integral process in the speed loop process is switched from complete integral to incomplete integral, so that the integral of the integrator of the speed loop is made incomplete integral only at the time of stop, thereby increasing the servo rigidity during operation. The output of the integrator is reduced so that the position does not overshoot only when stopped.

Action During normal operation, the speed loop integrator (integration process)
Performs perfect integration, so that the servo rigidity is kept high. On the other hand, when stopping, the integrator (integration process) is switched to incomplete integration from the distribution cycle one or more before the distribution cycle at which the movement command becomes “0”, and the movement command becomes “0”. When this happens, the value of the integral value should be small and overshoot should be avoided.

Embodiment FIG. 3 is a block diagram of a digital servo control device for carrying out an embodiment of the present invention, and has a schematic configuration because it has the same configuration as a conventional device for performing digital servo control.

In FIG. 3, reference numeral 20 denotes a numerical controller (hereinafter referred to as NC), reference numeral 21 denotes a shared RAM, reference numeral 22 denotes a digital servo circuit having a processor (CPU), ROM, RAM, etc., reference numeral 23 denotes a servo amplifier such as a transistor inverter, and reference numeral 24 denotes a servo amplifier. The servo motor 25 is a pulse coder that generates a pulse with the rotation of the servo motor 24.

NC20 shares movement command every IPT cycle (distribution cycle) RAM12
And the CPU of the digital servo circuit 21 reads this movement command from the shared RAM 12, and performs the position / velocity loop processing in a cycle obtained by dividing the ITP cycle into N pieces. NC20 per ITP cycle
Command Pc (n) in the position / velocity loop processing so that the movement command output from the controller is evenly distributed during the ITP cycle
From the difference between the movement command Pc (n) and the current position of the servo motor 24 obtained by the feedback pulse Pf of the position from the pulse coder 25 to obtain a speed command Vc. Command Vc and pulse coder
A speed loop process and a torque command (current command) Tc are obtained from the actual speed Vf of the servo motor 24 obtained by the feedback pulse from 25. Then, a current loop process is performed, a PWM command is created, and the servo motor 24 is driven via the servo amplifier 23.

FIG. 1 and FIG. 2 are flowcharts of the movement command reading processing for each ITP cycle, the position loop processing and the speed loop processing for each position / speed loop processing cycle, which are executed by the CPU of the digital servo circuit 22 in this embodiment. .

FIG. 1 is a flowchart of a process performed every ITP cycle. First, the CPU of the digital servo circuit reads the movement command Mc output from the NC 20 via the shared RAM 21 every ITP cycle (step S1). Then, it is determined whether or not this movement command is “0” (step S2). If it is “0”, it is determined whether or not the movement command one ITP cycle before stored in the register is “0” (step S2). S3) If "0", move instruction Mc read in step S1 is stored in register R (Mc) (step S3).
6). That is, the register R (Mc) stores 1
The movement command Mc before the cycle is stored.

If the movement command Mc read in step S1 is not "0", the flag F is set to "0", the counter CNT is set to "0" (steps S7, S8), and the process proceeds to step S6. As a result, as described later, the movement command M which is one cycle ahead of the movement command in the ITP cycle for performing the position / velocity loop processing is described.
When c is present, the flag F and the counter CNT are set to "0", and the flag F and the counter CNT are always set to "0" at the start of movement.

When the movement command Mc read in step S1 is "0" and the value of the register R (Mc) is not "0" (when stopping the movement), the flag F is set to "1". Then, the arbitrary value A set in the counter CNT is set (steps S4 and S5), and the process proceeds to step S6.

On the other hand, in the position / velocity loop period, the process shown in the flowchart of FIG.
(Mc) based on the movement command Mc stored in
A movement command for each speed loop processing cycle is obtained in the same manner as in the related art (step S100), and based on the movement command, a position loop process is executed in the same manner as in the related art to obtain a speed command Vc (step
S101). That is, since the position / velocity loop processing is performed based on the movement command stored in the register R (Mc), the reading of the movement command Mc for each ITP cycle shown in FIG. One ITP cycle read ahead.

Next, it is determined whether the flag F is "1" (step S10).
2) Since the flag F is set to “0” when the movement command Mc of the next ITP cycle is not “0”, the flag F is set at the start of movement or when movement has already been started. It is set to "0", and in this case, complete integration is executed in the speed loop processing. That is, a value obtained by subtracting the speed feedback amount Vf from the speed command Vc obtained in step S101 (speed deviation) is added to the integrated value Sv stored in the register, and the complete integration of the first equation for obtaining the integrated value Sv is executed. (Step S108), a torque command Tc is calculated by subtracting a value obtained by multiplying the integral value Sv by the integral constant k1 and a value obtained by multiplying the speed feedback amount Vf by the proportional constant k2 (Step S106). Delivered to loop processing (step S1
07), the position / velocity loop processing ends.

When the flag F is “1” in step S102,
That is, the movement command Mc of the ITP cycle one cycle ahead of the ITP cycle in the position / speed loop processing is “0” and the ITP cycle
When the cycle movement command is not “0” (stop movement 1I
Before TP cycle), the flag F is set to “1” and the counter CN
Since T is set to A (see steps S2 to S5), since the flag F is "1", steps S102 to S1 are performed.
In step 03, if the counter CNT is not "0" (not initially "0"), "1" is subtracted from the counter CNT (step S104), and the incomplete integration represented by the second equation is performed (step S104).
S105), a torque command Tc is obtained based on the obtained integral value Sv, and is passed to the current loop (steps S106 and S107). Less than,
As long as the position / velocity loop processing cycle flag F is "1", steps S100 to S100 are executed until the counter CNT becomes "0".
The processing of step 107 is executed, and in the integration processing of the speed loop processing, incomplete integration is executed.

As a result, since the position / velocity loop processing of the immediately preceding ITP cycle at which the movement command Mc of the ITP cycle becomes “0”, the integral processing of the speed loop processing is performed as an incomplete integration processing. The integral value Sv of the integration process in each speed loop process of the position / speed loop process period obtained by dividing the cycle into N becomes smaller sequentially, and the movement command in the position / speed loop process
When Pc becomes “0”, the integral value Sv becomes a small value, and the occurrence of overshoot at the time of stoppage is reduced.

When the value of the counter CNT becomes "0", the process proceeds to step S108, returns to the complete integration, and when there is a next movement command, the one before the execution of the movement command Mc is executed.
The flag F and the counter CNT are set to “0” in the ITP cycle (see steps S7 and S8).

In the above embodiment, the movement command Mc in the ITP cycle is used.
Although the integral processing of the velocity loop is set to the incomplete integration processing before 1ITP cycle before becomes zero, it may be switched from 2ITP cycles or 3ITP cycles to incomplete integration.

Advantageous Effects of the Invention The present invention makes the integral processing of the speed loop processing incomplete integration immediately before the servo motor stops its rotation, and completes the integration processing of the speed loop processing during normal servo motor driving. As a result, the servo rigidity is maintained, and the occurrence of overshoot at the time of stopping is reduced.

[Brief description of the drawings]

FIG. 1 is a flowchart of processing executed by a processor of a digital servo circuit for each distribution cycle (ITP cycle) according to an embodiment of the present invention, and FIG. 2 is a position / speed executed by a processor of the digital servo circuit in the embodiment. FIG. 3 is a block diagram of a digital servo controller which implements the embodiment, FIG. 4 is a block diagram of a position / speed control system of a servo motor, and FIG. FIG. 4 is an explanatory diagram illustrating that overshoot occurs when integration is used. 20 Numerical controller, 21 Shared RAM, 22 Digital servo circuit, 23 Servo amplifier, 24 Servo motor, 25 Pulse coder

──────────────────────────────────────────────────続 き Continued on the front page (58) Field surveyed (Int.Cl. ⁷ , DB name) G05D 3/12 G05B 19/404

Claims (1)

(57) [Claims] In a servo motor control method in which a processor executes position / velocity loop processing in response to a movement command instructed in each distribution cycle, a distribution cycle one or more times ahead of a distribution cycle set at that time. A control method for a servomotor, wherein when a movement command is "0", integration processing in a speed loop processing is switched from complete integration to incomplete integration in order to prevent overshoot.