WO1993024875A1

WO1993024875A1 - Control method for servomotor

Info

Publication number: WO1993024875A1
Application number: PCT/JP1993/000618
Authority: WO
Inventors: Yasusuke Iwashita
Original assignee: Fanuc Ltd
Priority date: 1992-05-22
Filing date: 1993-05-11
Publication date: 1993-12-09
Also published as: JPH05324086A

Abstract

This method is to reduce the positional errors by compensating for the delay in the rising torque of servo systems when the change in acceleration by an instruction is great. This control method comprises the steps of pre-smoothing (7) for each loop processing cycle of the position and speed; calculating the feed forward amount FFp on the basis of the pre-smoothed data SMD; and controlling the feed forward by adding the feed forward amount FFp to the speed instruction obtained by the positional loop. In the step of obtaining the feed forward amount FFp, the feed forward amount is calculated by advancing it temporally if the change in acceleration by the instruction is great, hence compensating for the delay in the rising torque.

Description

Specification

Servo motor control method

Technical field

The present invention relates to a control method of a servomotor that drives a feed shaft of a machine tool or an arm of a robot, and particularly to a feedforward control method of a servomotor.

Background technology

When a servomotor is used to control the feed axis of a machine tool or the arm of a robot, especially when performing high-speed cutting with a machine tool, a shape error occurs due to the delay in following the servo system. Cheap. Therefore, feed-forward control is performed in the position loop in order to reduce the position deviation and correct the servo delay.

This feed-forward control differentiates the movement command at each position and speed processing cycle, and multiplies the derivative by the feed-forward coefficient to obtain a normal position loop. The speed command obtained by the processing is added to and corrected, and the speed loop processing is performed based on the corrected speed command.

The distribution cycle (ITP cycle) in which the movement command is transferred from the numerical controller to the servo circuit side, that is, to the position loop, is typically about 8 msec, and the position loop and speed inside the servo circuit are The loop period is typically 2 msec or lmsec. Then, in the position loop, the ITP cycle in which the movement command is passed from the numerical controller is divided into the position loop cycle, and each of the divided position loop cycles is divided. It is controlled so that the movement command in is equal. Therefore, even if the acceleration / deceleration time constant is given to the movement command output from the numerical controller and output, the position loop in each position and speed loop processing cycle in each ITP cycle Since the movement commands in the position loop processing are controlled so as to be even, a large step is generated in the movement commands in the position loop processing cycle before and after the ITP cycle shifts, and this is a feed forward. Differentiated by the C term, it becomes a large value. As a result, the speed command contains a high-frequency component, which makes it impossible to follow the speed lobe, causing undulations in the position deviation, and greatly affecting the movement of the motor and the machine. O

Therefore, in order to solve the above problem, acceleration / deceleration processing was introduced into the position control and also the speed control in the feedback field to eliminate the undulation. Method of controlling a servomotor for performing a smoothing operation (smoothing process) to be performed The inventor of the present invention filed an international application PCTZJP 90/0380 to file an international publication WO900Z1. It was published internationally as 156-2.

The inventors of the present application have made further improvements, and in the above-mentioned smoothing operation that averages past data in time, the data is output later (future) than the ITP cycle. The preceding smoothing process is performed by using a leading smoothing process for averaging the position and speed loop cycle movement commands before and after the position and speed loop cycle movement command using the movement command. A control method was developed in which the value obtained by the feedback processing was added to the speed command obtained by the ordinary position loop processing as a feed-forward amount, and the international application PCTZJP 9.1 Z 0 15 3 Issue No. 7, PCTZJP 92 0 0 66 66 and PCT / JP 92/0 1 150, respectively, International publication numbers W 0 9 0 9 0 2 2 and W 0 9 2 2 1 It was published internationally as No. 075 and W093 / 054545.

In this preceding smoothing processing, the following equation (1) is calculated to obtain the average value m of the movement command, and the average value m is multiplied by the feedforward coefficient α to obtain the position value. The feed amount is assumed to be the amount.

m = Z ^d (l + Z one ¹ dozen ^{... + Z- - 1)) ·} ( motion command amount) N

... (1) Note that N is the value obtained by dividing the ITP cycle by the cycle of the position and velocity loops, and Z- ¹ is the delay of the processing cycle of one position and velocity loop. Z ^d is an advancement element, and the advance amount d is about 12 of the above N, and is set to “3” or “4” when N = 8, and d = l when N = 4 Or set to 2.

Also, in the above PCTZJP92Z06666 and PCTZJP92 / 01150, the preceding smoothing processing for obtaining smoothing data equivalent to the above equation (1) is performed. A device that uses the movement command of the ITP cycle and the preceding and following ITP cycles is also proposed.

Even when the feed-forward control including the preceding smoothing process is performed, the change in the acceleration of the command is large. In this case, the rise of the torque of the servomotor is delayed, and a position error is likely to occur. The acceleration change of the command can be considered as the change of the difference of the movement command for each ITP cycle. That is, since the movement command for each ITP cycle is a movement command in a predetermined time (ITP cycle), it means speed. Therefore, the difference between the movement commands for each ITP cycle means the acceleration, and the change in the difference between the movement commands indicates a change in the acceleration of the command.

Disclosure of the invention

An object of the present invention is to compensate for a delay in the rise of a torque of a servo system, which is likely to occur when a command change in acceleration is large, and to reduce an error in an actual movement locus.

According to the servo motor control method of the present invention, a velocity command is obtained by performing a position loop process based on a movement command distributed from a numerical controller in each distribution cycle and a position feedback amount. A step of detecting the magnitude of the command acceleration change from the change amount of the movement command in the distribution cycle and the distribution cycle before and after the distribution cycle; and a step in which the magnitude of the command acceleration change is predetermined. A step of determining whether or not the value is equal to or greater than a predetermined value; a step of calculating a feed-forward amount of the position based on a movement command of the distribution cycle and distribution cycles before and after the distribution cycle; and When the magnitude of the acceleration change is equal to or larger than a predetermined threshold, the position feed-off position is smaller than when the magnitude of the acceleration change is smaller than the predetermined threshold. Advance the amount of time by the specified time. And a child and out, position Lou A step of correcting the speed command by adding a feed-forward amount of the calculated position to the speed command obtained by the speed processing.

According to one aspect of the present invention, the step of calculating the feedforward amount includes a step of performing a preceding smoothing process to obtain smoothing data for each position loop processing cycle. SMD 0 indicates the smoothing data of the current position loop processing cycle, SMD 1 indicates the smoothing data of the current position loop processing cycle, and indicates the feed-forward coefficient. A step of obtaining the position feed-forward amount FF p by performing the operation of the following equation (2), where

F F p = α {k · S M D 0+ (1-k) S M D l}

(2) The value of parameter k is set to k1 if the magnitude of the acceleration change is smaller than a predetermined threshold value, and to k1 if the magnitude of the acceleration change is equal to or larger than the predetermined threshold value. Includes a step of setting a value of k2 larger than k1 for a predetermined period.

According to another embodiment of the present invention, the position feed-amount FF p can be obtained by the following equation (3). In this case, when the magnitude of the acceleration change is smaller than the set value, the value of the parameter k is set to kl, and when the magnitude of the acceleration change is equal to or larger than the set value, the set time is set. The value of parameter k is determined by setting k3 to a value smaller than k1 by a predetermined amount.

F F p = α {(1-k)-S M D O + k-S M D 1}

(3) By calculating the feedforward amount as described above, the speed command is compensated ahead of time by the amount corresponding to the delay of the servo system, the response to the command is improved, and the movement is improved. Trajectory error is reduced.

BRIEF DESCRIPTION OF THE FIGURES

FIG. 1 is a block diagram of a servo system for performing feedforward control of the present invention.

FIG. 2 is a block diagram of a servomotor control device for performing the method of the present invention.

Fig. 3 is a flowchart of the processing performed by the processor of the digital servo circuit shown in Fig. 2 every ITP cycle, and Fig. 4 is the position and position of the processor of the digital servo circuit shown in Fig. 2. The flow chart of the processing executed for each speed loop processing cycle,

FIG. 5 is an explanatory diagram of a process for obtaining smoothing data from a movement command,

FIG. 6 is an explanatory diagram of a process of calculating a feedforward amount,

Fig. 7 is a diagram showing the cutting trajectory when shifting from straight line machining to arc machining by the conventional feedforward control method.

FIG. 8 is a diagram showing a cutting trajectory when shifting from linear machining to circular arc machining by the feedforward control method of the present invention.

BEST MODE FOR CARRYING OUT THE INVENTION In Fig. 1, DDA (igital Differential Analyzer) 1 uses a position command and a speed loop to send a movement command M cmd sent from a CNC (numerical control device with built-in computer) for each distribution cycle (ITP cycle). Divide into movement instructions for each processing cycle. The error counter 2 calculates the position deviation by adding the movement command output from the DDA 1 and reducing the movement amount of the servo motor for each position and speed loop processing cycle.

Numeral 3 is a term for obtaining a speed command by multiplying the position deviation stored in the error counter 2 by the position gain Kp. 4 is a term of the velocity loop, where kl is an integral constant and k2 is a proportional constant. 5 and 6 are the terms of the transfer function of the servomotor, Kt is the Tonnell constant, and Jm is the motor inertia.

10 and 11 are the terms of the transfer function of the machine coupled to the servomotor, K m is the panel constant, C m is the viscous term, and J L is the machine inertia. 1 2 is a term of a transfer function for converting speed into position.

7 is the term of the preceding smoothing process, and 8 is the feed-forward amount FF p of the position from the preceding smoothing data SMD obtained by the preceding smoothing process. Where k is the characteristic of the machine connected to the servomotor and the parameter that is adjusted according to the change in acceleration, which is described later.Nana is the feed-forward coefficient of the position. is there. 9 is a term for differentiating the preceding smoothing data SMD, that is, a term for obtaining the velocity feedforward amount FFv of the speed from the smoothing data of the current cycle and the smoothing data of the previous cycle. It is. Note that T s is the period of the position and velocity loops.

At each ITP period, a movement command Mcmd is output from a numerical control device such as CNC, and DDA1 obtains a movement command for each position and velocity loop processing period Ts. The position error is obtained by the error register 2 from the movement command and the feedback amount of the position, and the position error is multiplied by the position gain Kp to obtain the speed command Vc1. Can be On the other hand, the leading smoothing means 7 performs processing based on the processing of the above equation (1) or based on the movement command Mcmd of the ITP cycle and the movement command Mcmd of the ITP cycle before and after the ITP cycle. Obtain smoothing data SMD.

If the change in the acceleration of the command is smaller than the threshold value A using the preceding smoothing data SMD, the position feed-forward is calculated by the following equation (4). When the command acceleration change is equal to or greater than the threshold value A, the position feed-forward is calculated for a predetermined period by the calculation of the following equation (5). Calculate the load amount FF p (processing in item 8).

When the command acceleration change is small,

FFp = {k a + (1-k a) Z "

= a {k a * S M D 0+ (l-k a) · S M D 1}

( Four )

When the acceleration change of the command is large,

FFp = a {(ka + kb) + (1-ka — kb) Z " ^a } · SMD = {(ka + kb) SMD0 + (1-ka-kb) SMD1}

( Five ) Note that SMDO indicates the preceding smoothing data calculated in the position and speed loop processing cycle, and SMD 1 indicates the preceding smoothing data calculated one position and the speed loop processing cycle earlier. When the acceleration change is equal to or greater than the threshold value A, the feedforward amount FFp is equal to the preceding smoothing of the cycle, compared to when the commanded acceleration is smaller than the threshold value A. Since the data increases by SMDO kb and decreases by the preceding smoothing data SMD 1 kb calculated one cycle Ts before, the feed-forward amount FFp is a predetermined time corresponding to kb. Only in time.

Then, the position feed-forward control of the position is performed by adding the feed-forward amount FFp of the position to the speed command Vc1 obtained by the position loop processing. Find the speed command V c at which was executed. From this speed command Vc, the speed feedback amount from the servomotor is subtracted to determine the speed deviation, and the same speed loop processing as in the past is performed to obtain the torque command Tc1. In addition, the value obtained by differentiating the preceding smoothing data SMD (obtaining the differential value by subtracting the preceding smoothing data of the previous cycle from the preceding smoothing data of the cycle) to the value obtained by calculating the servo motor torque. Constant K t, inertia J m, constant (J m + JL) K t determined by inertia JL of the machine coupled to the servomotor, and velocity feed-forward Multiply by 5 to find the speed feedforward amount FFv. That is, the performance of the following equation (6) Calculation to find the speed feed-forward amount FFV.

F F V = {^ (J m + J L) / K t} {(1-Z-/ T s} S M D

= {β '(J m + J L) / K t} (S M D 0- S M D l)

(6) This feed-forward amount FFV is added to the torque command Tc1 obtained by the above-described speed loop processing, and the speed feed-forward control is performed. The executed torque command Tc is obtained, and the servomotor is driven by the torque command Tc.

FIG. 2 is a block diagram of a servomotor control system to which the present invention is applied. The configuration is schematically shown because the configuration is the same as that of a conventional device for performing digital servo control.

In FIG. 2, 20 is a computer numerical controller (CNC), 21 is a shared RAM, 22 is a digital servo circuit having a CPU, ROM, RAM, etc., and 23 is a transistor driver. , M is a servomotor, and 24 is a pulse coder that generates pulses with the rotation of the servomotor M. FIG. 2 shows only a single-axis servo motor.

The CNC 20 writes the movement command Mcmd into the shared RAM 21 for each ITP cycle (distribution cycle), and the CPU of the digital servo circuit 22 reads the movement command Mcmd from the shared RAM 21 and executes the above ITP cycle. Position and velocity loop with a period T s (ITP period = T s XN) Perform processing. In each ITP cycle, a movement command in the position and velocity loop processing T s is determined so that the movement command Mcmd output from the NC 20 is evenly distributed during the ITP cycle (DDA processing). The position loop processing is performed based on the difference from the current position of the servomotor M obtained by the feed knocking pulse from the pulse coder 24, and the position of the servo motor M is described later. Performs speed control processing to obtain a speed command. Next, speed loop processing is performed from the actual speed of the servo motor M obtained by the obtained speed command and the pulse coder 24 force, the feed-nod valve, and the like. Performs speed feed-forward processing and finds the torque command (current command). Then, current lube processing is performed, a PWM command is created, and the servo motor M is driven via the servo amplifier 23.

Next, the CPU of the digital servo circuit 22 performs

The processing performed for each ITP cycle will be described with reference to FIG.

First, in step S1, the CPU of the digital servo circuit 22 reads the movement command Mcmd distributed from the CNC 20 from the shared memory 21 and sends it to step S2. At this time, the value stored in register R2 is stored in register R3, the value stored in register R1 is stored in register R2, and the movement read in step S1 is stored in register R1 Stores the command M cmd.

As described below, the normal position and velocity loop processing is Since execution is performed based on the value of the movement command Mcmd stored in the register R2, one movement command Mcmd is read ahead. Register R 1 has a (future) movement command M cmd that is temporally advanced by one ITP cycle with respect to the movement command M cmd for processing the position and velocity loops. Means that the movement command Mcmd of the position and speed loop processing cycle is stored in the register R3, and the (past) movement command Mcmd delayed by one ITP cycle is stored in the register R3. . Note that “0” is stored in registers R1, R2, and R3 by default.

In step S3, the value of the counter C1 indicating the index of the position and speed loop processing cycle is set to "0".

In step S4, first, the value stored in the register R2 is subtracted from the value stored in the register R1, and the movement command Mcmd of the ITP cycle and one ITP cycle ahead ( Find the difference between the future movement commands M cmd. This difference represents the acceleration between the current cycle and the next cycle. That is, since the movement command distributed for each ITP cycle is a movement command for a predetermined time (ITP cycle), it represents the speed, and the difference between the cycle and the next cycle movement command (speed) represents the acceleration. And Similarly, the value stored in the register R3 is subtracted from the value stored in the register R2, and the acceleration in that cycle is calculated one ITP cycle before (in the past). The absolute value of the difference I (R 1-R 2)-(R 2-R 3) Determines whether I is greater than or equal to the set threshold A If the absolute value of the acceleration difference is equal to or greater than the threshold value A, the set value B is stored in the force counter C2, and the processing of the ITP cycle ends. If the difference between the accelerations is smaller than the threshold value A, the process ends without setting the counter C2. That is, the counter C 2 is set to the value of B only when the acceleration change is equal to or more than the threshold value A. Hereinafter, this process is executed in each ITP cycle. The value of B above is a positive integer value set corresponding to the time (usually about 20 to 30 msec) that the servo system delays with respect to the change in acceleration.

Next, processing executed by the CPU of the digital servo circuit 22 at each position and speed loop processing cycle Ts will be described with reference to the flowchart of FIG.

In the CPU of the digital servo circuit 22, in step A 1, the value of the counter C 1 is smaller than 2 of the number of divisions N (= ITP cycle position and speed loop processing cycle) It is determined whether or not. If the value of the counter C1 is smaller than NZ2, the value of the register R2 is subtracted from the value of the register R2 in the accumulator SUM in step A2. If the value of the counter C is NZ2 or more, the value of the register R2 is subtracted from the value of the register R1 to the accumulator SUM in step A3. Then, the process proceeds to step A4. Note that the value of the accumulator SUM is set to "0" in the initial setting.

Increase the value of the accumulator SUM in step A4. Divide by the square of the number of divisions N to obtain the preceding smoothing data SDM0. Next, the counter C1 is incremented (step A5). O The processing of steps A1 to A5 is the processing for obtaining the preceding smoothing data SDM0. is there.

The preceding smoothing processing based on the above-mentioned equation (1) is to obtain the preceding smoothing data SDM0 using the movement command for each position and speed loop (the movement command after DDA processing). Was. This method complicates the processing, but in the preceding smoothing processing of the processing in steps A1 to A5, relatively simple processing is performed from the movement command Mcmd for each distribution cycle. It is possible to obtain the preceding smoothing data based on the theory. It will be described later that the preceding smoothing data obtained by the above equation (1) is substantially the same as the preceding smoothing data obtained by the processing of steps A1 to A5.

The process of obtaining the preceding smoothing data based on the equation (1) is described in detail in PCTZJP 9 I / O 1537 and PCTZJP 92/0 1150 described above. I have.

Next, in step A6, it is determined whether or not the value of the counter C2 is positive. If the counter C2 is not positive (if it is “0”), the process proceeds to step A7, and the smoothing data SMD0 and the register obtained at the position and speed loop processing cycle are obtained. From the smoothing data SMD 1 one cycle before stored in the data, the above formula (4) is calculated, and Find the position feed-forward amount FF p.

If the set value B is stored in the counter C2 in step S5 described above, and the current value of the counter C2 is positive, the counter C2 is set in step A8. After subtracting “1” from the counter C 2, the arithmetic operation of the above equation (5) is performed in step A 9 to obtain the feed-off head amount FF p of the position.

Next, the process proceeds to step A10 to perform DDA processing based on the movement command Mcmd stored in the register R2, thereby obtaining a movement command for the position and speed loop processing cycle.

Note that during the first ITP1 cycle at the start of operation, the movement command Mcmd is not stored in the register R2, so the movement command of the position and speed loop is "0". In other words, only the smoothing processing and the feed-forward processing are executed in advance of one ITP cycle. This means that the feed-forward processing is performed one ITP period before the processing of the position and velocity loops.

Based on the position and speed loop processing cycle movement command obtained in step A10 and the feedback amount at the position from the pulse coater 24, the same position loop as before is performed. The speed command Vc1 is obtained by performing the step processing (step A11), and the speed command Vc1 is fed to the position feedthrough of the position obtained in step A7 or step A9. The speed command Vc that has been subjected to the position feedforward processing is obtained by adding the mode amount FFp (step A12). This speed command Vc and speed feedback signal enable the same speed loop processing as before. To obtain the torque command Tc1 (Step A13) _c. Further, the smoothing data SMD0 obtained in the cycle and the smoothing data of the previous cycle stored in the register are obtained. The calculation of the above equation (6) is performed by the SMD 1 to determine the feedforward amount FFv of the speed (step A14). This speed feedforward amount FFV is added to the torque command Tel obtained in step A10, and the torque command Tc under speed feedforward control is added. (Step A15), and passes the torque command Tc to the current loop (Step A16). Then, the smoothing data SMD0 obtained in the cycle is stored in a register so as to be used as the smoothing data SMD1 one cycle T s before in the next cycle (step S10). Step A 17), terminate the processing of the position and speed loop. Hereinafter, the CPU executes the processing described in FIG. 4 for each position and speed loop processing cycle T s.

In each of the above embodiments, the example in which the speed feedforward control is also performed has been described. However, the speed feedforward control need not always be performed. .

Next, with reference to FIG. 5, it will be described that a method of obtaining the preceding smoothing data is substantially the same as the above-described processing method and a method of obtaining the same by the equation (1). .

An example will be described in which the position and velocity periods are four times during the ITP period, that is, N = 4, and the advance amount d in Equation (1) is 2. The movement command M cmd for n cycles of the ITP cycle is Q n, and when n ≤ 0, Q n = 0 and n ≥ l And movement commands Ql, Q2, ..., Qn, ... are distributed. Also, the respective q 1. q _2, q ₃ motion commands of each in the move command to the ITP period Q l, Q2, Q 3 and 4 divided position and velocity loop. Since the movement commands in the position and speed loops in the DDA are to be made uniform in each cycle, Qn / N = QnZ4 = qn. Then, the values of the counter Cl, the accumulation sum SUM, and the preceding smoothing data SMD0 in the above steps A1 to A5 are as shown in Table 1 below.

As can be seen from Table 1, the preceding smoothing data SMD with n ITP cycles is

In the first position and velocity loop period,

(q »-! + 3 q„) / 4

In the second position and velocity loop period,

Q n

In the third position and velocity loop period,

(3 q „+ q„ ₊ ι) Z 4

In the fourth position and velocity loop period,

_{_{(2 q n + 2 q n}} + i) / 4

Therefore, this is obtained by adding the movement command of the position and speed loop cycle, the previous (past) movement command, the next (future) movement command, and the second movement command and adding 4 In other words, it indicates that the preceding smoothing data SMD is required.

On the other hand, the preceding smoothing data SMD obtained by the equation (1) is represented by the following equation (7).

SMD = Z ² (l + Z-'+ Z-' + Z " ³ ) q, / 4

= (Ζ ² + Ζ ¹ + 1 + Ζ- ¹ ) qj / 4 = {q (j + 2) + q (j + l) + q (j) + q (j-1)} Z 4

(7) In the above equation (7), q (j) indicates the movement command for the j-th position and velocity loop cycle. As can be seen from equation (7), The preceding smoothing data SMD includes the movement command q (j) of the position and velocity loop of the cycle j, the movement command q (jl) of the immediately preceding (past), and the movement of the preceding (future). It shows that the command q (j + l) and the movement command q (j + 2) two ahead are added and the result is divided by 4, and the preceding sum calculated by the above equation (7) is obtained. Switching data SMD and step A above

The preceding smoothing data SMD obtained in 1 to A5 indicates that they are the same.

Therefore, if the feed-forward quantity FF p of the position is expressed by using the above equation (7), the preceding smoothing data SD M0 at the point of the position and velocity loop period j is Expression

(7)

S M D 0 = {q (j + 2) + q (j + l) + q (j) + q (j-1)} / 4

(8) The preceding smoothing data S DMI one cycle (j-1) before the cycle j is given by the following equation (9).

S M D 1 = {q (j + 1) + q (j) + q (j-1) + Q (j-2)} / 4

… (9) Thus, the position feed-off amount F F p is given by o

F F p = α {k-S M D 0+ (1-k) S M D 1}

= (/ 4) [k {q (j + 2) + q (j + 1) + q j + q (j-1)} + (1- k) (q (j + l) + q (j) + q (jl) + q (j-2)}]

= (a / 4) [{q (j + 1) + q (j) + q (j-1) + q (j-2)} + k {q (j + 2)-q (j-2) }]

… (10) Then, as shown in Fig. 6, assuming that the position and velocity loops are j periods and are the periods immediately after the switching of the ITP period, The register Rl receives the movement command Q3 one ITP cycle ahead (future), the register R2 receives the movement command Q2 of the ITP cycle corresponding to the position and speed loop, and The register R 3 stores the movement command Q 1 after (past) one ITP cycle. In this example, Q1 = Q2 and ql = q1.

The feedforward amount FFp at the j-th position and velocity loop cycle is given by the above equation (10).

FF p = (a / 4) {q ₂ + q 2+ q i + q i + k (q ₂ -qi)}

= Q 2

... (1 1) Similarly the (j + 1) th position and speed loop period, FF p = (a / 4 ) {q 2 + q z + q 2+ q i + k (q 2 - q 1)}

= Q 2

At the (j + 2) th position and velocity loop period,

FF p = (α / 4) {q ₂ + q 2 + q 2 + q 2 + k (q ₃ — q ₂ )}

= (/ 4) {4 q 2 + k (q a- q ₂ )}

At the (j + 3) th position and velocity loop period,

F F p = (α / 4) {q a + q 2 + q 2 + q z + k (q 3-q 2)}

= (a / 4) 3 + 3 q ₂ + k (q ₃ — q ₂ )}.

When the movement command of Q3 is smaller than Q1 = Q2 and the change in acceleration of the command is smaller than the threshold value A, the value of k is ka. Above the threshold value A, k = ka + kb, which depends on whether the acceleration change of the command is large or small. Therefore, the feedforward amount FF p is calculated by the period j and the period

There is no change when (j + 1), but a change occurs when the period (j + 2) and period (j 3). That is, depending on whether the acceleration change of the command is large or small, the feedforward amount FFp is k b at the periods (j + 2) and (j + 3).

(q „-q ₂ ) This occurs when the movement command Q 3 one ITP cycle ahead (future) is large, and therefore the acceleration change is small when the acceleration is large. In contrast, the feedforward amount is increased by kb (qa-q2), and when the speed is greatly reduced, only Ikb (q3-q2) I This means that the feed amount is reduced, which means that the feed-forward control is advanced in time.

Also, the feedforward amount F F p can be obtained by the following equation (13). In this case, when the acceleration change of the command is smaller than the threshold value T, the value of k is set to ka, and when the acceleration change of the command is larger than the threshold value T, ¾: = 1 ^ The feedforward amount FFp is obtained as 3 — 1 ^ (;. In this case, the same operation and effect as those of the above embodiment can be obtained.

F F p = α {(Ι-k) · S M D O + k · S M D 1}

… (1 3) When the preceding smoothing data SMD is obtained, when the number N of the position and speed loop processing cycles for one ITP cycle is an even number such as 4 or 8, for example, when N = 4 The value of d is "1" or "2", and when N = 8, the value of d is "3" or Since is set to “4”, it is not possible to take an average around the position and speed loop processing cycle. Therefore, the leading smoothing data SMD will be delayed or advanced in time. Therefore, as described in PCTZJP92 / 0150, advanced advanced smoothing data is obtained (when N = 4, d = 2). By calculating the weighted average of the data and the preceding smoothing data obtained from the previous position and speed loop cycle, the preceding smoothing data with no time delay and advance is equivalently obtained. It is also possible to obtain more feedforward amount.

FIGS. 7 and 8 are diagrams showing the results of an experiment for investigating the effect of the present invention. When cutting an arc having a radius of 32 mm, the moving speed is constant and the Y axis reaches the point P. The cutting trajectory when a linear command is issued along the direction and the circular command 30 is given from the point P is drawn. However, in order to represent the error of the actual trajectory with respect to the command, the actual trajectory is enlarged in the radial direction with respect to the actual trajectory. Therefore, the trajectory 31 of the straight part is not shown parallel to the Y axis.

FIG. 7 shows a conventional feedforward control. When the acceleration change of the command is large as in the present invention, the feedforward command is not advanced in time. The results are shown in the case where the control is performed by obtaining the feedback amount regardless of the acceleration change. FIG. 8 shows the experimental results when the present invention was implemented. When changing from point P to an arc command, the speed in the X-axis direction It increases sinusoidally from “0”, and decreases sinusoidally from the previous speed (command speed) in the Y-axis direction. That is, the change in acceleration of the command changes sharply at point P. As can be seen by comparing FIGS. 7 and 8 with the actual trajectory at the point P where the change in acceleration is large, the error of the actual trajectory with respect to the command is smaller in FIG. You can see that it is done.

As described above, according to the present invention, when the acceleration change of the movement command in the ITP cycle in which the position and velocity loop processing is performed is large, the feed-ahead amount is temporally reduced. It is calculated by proceeding. As a result, the feedforward amount is increased in the forward and reverse directions according to the acceleration change of the command, and the torque command by the position and velocity loop processing is increased in the forward and reverse directions. As a result, the delay of the rise of the servo system is compensated. As a result, the position error is reduced even when the moving trajectory of the machine tool or the robot driven by the servomotor changes abruptly.

Claims

The scope of the claims

1. (a) A step of obtaining a speed command by performing position loop processing based on the movement command distributed from the numerical controller at each distribution cycle and the position feedback amount,

(b) detecting the magnitude of the command acceleration change from the change amount of the movement command in the distribution cycle and the distribution cycle before and after the distribution cycle; and

(c) a step of determining whether or not the magnitude of the change in acceleration of the command detected in step (b) is equal to or greater than a predetermined threshold value;

(d) a step of calculating the feedforward amount of the position based on the movement command of the distribution cycle and the distribution cycle before and after the distribution cycle; and

When the magnitude of the acceleration change is equal to or greater than a predetermined threshold value according to the determination result in the step (c), the magnitude of the acceleration change is determined. (E) calculating the feedforward amount of the position at a position smaller than the predetermined threshold value by advancing the feedforward amount of the position by a predetermined time. ), And a step of adding the feedforward amount at the position calculated in step (d) to the speed command obtained in step (d) to correct the speed command. Control method.

2. The step (d) includes: (d1) a step of performing a preceding smoothing process to obtain an entire smoothing data every position loop processing cycle; (d2) SMD0 is the smoothing data of the position loop processing cycle obtained in the above step (dl), SMD1 is the smoothing data before the first position loop processing cycle, and feed-forward. A step of calculating the feed-forward amount FF p of the position by performing the following equation with the word coefficient and the parameter being k;

F F p = a {k-S M D O + (l-k) S M D l}

(d3) The value of the parameter k is set to k1 when the magnitude of the acceleration change is smaller than a predetermined threshold value, and the value of the parameter k is set to a predetermined threshold value. 2. The servo motor control method according to claim 1, further comprising: setting a value of k2 larger than k1 for a predetermined period.

3. (f) a step of performing a speed loop process based on the speed command and the speed feedback amount to obtain a torque command,

(g) calculating a velocity feed-forward amount based on a value obtained by differentiating the smoothing data obtained in the step (d 1);

(h) The torque command is corrected by adding the feedforward amount of the speed calculated in the step (g) to the torque command obtained in the step (f). 3. The control method for a servomotor according to claim 2, further comprising a step of performing the operation.

4. The step (d) is (d4) preceding smoothing Performing a process to obtain a smoothing delay for each position loop processing cycle; and

(d5) SMD0 is the smoothing data of the position loop processing cycle determined in the above step (d4), SMD1 is the smoothing data of the previous position loop processing cycle, and feed When the power coefficient is α, no, and the parameter is k, the following formula is used to calculate the position feed-forward amount.

Steps to find F F p

F F p = α {(Ι-k) · S M D O + k-S M D 1}

(d6) The value of the parameter k is set to k1 when the magnitude of the acceleration change is smaller than a predetermined threshold value, and the value of the parameter k is set to a predetermined threshold value. 2. The servo motor control method according to claim 1, further comprising: setting a value of k3 smaller than k1 for a predetermined period.

5. (f) A step of performing a speed loop process based on the speed command and the speed feedback amount to obtain a torque command,

(g) calculating a velocity feed-forward amount based on a value obtained by differentiating the smoothing data obtained in the step (d4);

(h) The torque command is corrected by adding the feedforward amount of the speed calculated in step (g) to the torque command obtained in step (f). A method for controlling a servo motor according to claim 4, comprising a step of performing