METHOD FOR CONTROLLING SERVOMOTOR FEEDFORWARD

Abstract: A feedforward controlling method for driving a servomotor smoothly and stably by improving the follow-up ability to a command of a servosystem. The method can be adapted to servosystems in which a position and speed loop processing is repeated plural times during a period for distributing the movement commands of a host controlling device. A temporary speed command, which is the product of the gain (Kp) and the deviation (e) between a movement command (a) and an actual rotational quantity (Pf) of the motor in each period of the position and speed loop processings, is calculated. A positional feedforward quantity, which is the product of a factor (α₁) and the average value of the movement command in each processing period and the movement commands in the periods before and after the processing period, is calculated. Then, a speed command is calculated by correcting the temporary speed command with the positional feedforward quantity. Further, a temporary torque command is calculated in the speed loop which has an integration term (2) and a proportion term (3). A speed feedforward quantity, which is the product of a factor (α₂) and the differentiated value of the value which is obtained by lead-compensating the average value of the movement commands with a lead compensating element (8) by a time equal to a multiple of the processing period by a predetermined number. Then, a torque command for driving the servomotor (4) is obtained by correcting the temporary torque command with the speed feedforward quantity.

Description: This invention relates to a feedforward control method permitting smooth and stable operation of a servomotor. Background Art In servo systems in which servomotors installed in various machines, such as machine tools and robots, as drive sources are controlled in such a manner that the deviation of an actual motor position from a commanded motor position becomes zero, the position deviation increases if there is a follow-up delay with respect to the position command. This system, however, has a disadvantage that, for example, the shaping error tends to occur in the operation using machine tool, and such an error is very likely to occur particularly during a high-speed machining operation in which the position command changes rapidly. Therefore, in servo systems wherein a position/speed loop process is executed at intervals shorter than predetermined pulse distribution intervals in accordance with a move command which is supplied from an upper-level control device at the pulse distribution intervals, a differential value of the position command is added to the speed command corresponding to the position deviation to provide a phase-lead compensation or a position feedforward control on the position command, thereby eliminating the response delay of the servo system with respect to the position command. In this type of servo systems, when the move command from the upper-level control device changes stepwise, an averaging of move commands will be made among the periods of the position loop processes belonging to one pulse distribution period, but the move command can change stepwise between the last position loop processing period in one pulse distribution period and the first position loop processing period in the subsequent pulse distribution period. More particularly, the speed command that has been subjected to the phase-lead compensation contains more high-frequency components than that on which no such compensation is performed. On the other hand, in conventional servo systems, the speed control loop for providing a current command corresponding to the deviation of an actual speed from the speed command does not have an adequate response to high-frequency components in the speed command. Accordingly, in conventional servo systems, the position deviation undergoes wavy changes. Even if an acceleration/deceleration control is applied to the position command to make the position command change smoothly, a wavy change of the position deviation may still occur. As a result, a shock will be given to the servomotor and thus to a mechanical system using the motor as the drive source. To eliminate such a drawback, a feedforward control device has been proposed (Japanese Patent Application No. 1-150481) in which a feedforward controlled variable, obtained by differentiating the position command, is subjected to an acceleration/deceleration process and then is added to a controlled variable obtained by the position control loop to obtain a speed command. This device not only can eliminate high-frequency noise components contained in the feedforward controlled variable but also can considerably reduce wavy changes of the position deviation. Nevertheless, the smoothing process used in this type of feedforward control devices for reducing wavy changes of the position deviation involves averaging past data, and therefore, the feedforward controlled variable is subject to a time lag with respect to the command. As a result, still wavy changes of the position deviation cannot always be sufficiently reduced. Disclosure of the Invention An object of this invention is to provide a feedforward control method having an improved follow-up characteristic with respect to a command of a servo system and thus to enable a servomotor to operate smoothly and stably. To achieve the above object, this invention provides a feedforward control method for a servo system in which a position-speed loop process is periodically carried out plural number of times within one distribution period in accordance with a move command periodically distributed from an upper-level control device, comprising the steps of: (a) executing a position loop process in each position/speed loop processing period in accordance with a corresponding move command to calculate a provisional speed command; (b) calculating a mean value of move commands corresponding to the each position/speed loop processing period and those corresponding to said each position/speed loop processing periods before and after the each position/speed loop processing period; (c) calculating a feedforward controlled variable in accordance with the mean value of the move commands; and (d) correcting the provisional speed command by using the calculated feedforward controlled variable to obtain a speed command. As described above, according to this invention, in each position/speed loop process, not only the move command for present processing period but also the move command for the subsequent position/speed loop processing period are used when calculating the feedforward controlled variable, and the speed loop process is carried out in accordance with a speed command corrected by the feedforward controlled variable, whereby the follow-up characteristic with respect to the commands for the servomotor can be improved. Accordingly, even when the move command supplied from the upper-level control device to the servo system changes stepwise, the position deviation does not undergo a wavy change, and therefore, the servomotor and a machine using the servomotor as a drive source can be smoothly and stably operated. Brief Description of the Drawings FIG. 1 is a schematic block diagram of a servo system for carrying out a feedforward control method according to an embodiment of this invention; FIG. 2 is a schematic block diagram of a digital servo control device corresponding to the servo system shown in FIG. 1; FIG. 3 is a flowchart of a move command reading process to be executed by a processor of the digital servo control device; FIG. 4 is a flowchart of a position/speed loop process to be executed by the processor; FIG. 5 is a graph showing an example of move commands and position feedforward quantities in each of successive position/speed loop processing periods; FIG. 6 is a graph showing an example of a change of a position deviation in a servo system for carrying out a conventional feedforward control, observed when a ramp input of the move command is made to the servo system; FIG. 7 is a graph showing an example of a change of the position deviation observed when a lead quantity for a speed feedforward control is set to zero and the ramp input of the move command is made, according to the embodiment of this invention; and FIG. 8 is a graph similar to FIG. 7, obtained when the lead quantity is set to "2". Best Mode of Carrying Out the Invention Referring to FIG. 1, a servo system comprises a position loop represented by a transfer element 1 having a position gain Kp, and a speed loop represented by a transfer element 2 corresponding to an integral term of the speed loop and a transfer element 3 having a proportional gain k2. Symbol k1 denotes an integral gain, Ts a position/speed loop processing period, and Z⁻¹ a Z-transform for delaying a speed command by a time period corresponding to one position/speed loop process. A transfer element 4 corresponds to the mechanical parts of a servomotor, and symbols Kt and Jm represent a torque constant and inertia respectively. A transfer element 5 corresponds to a position detector for detecting the rotational position of the servomotor. A transfer element 6, comprising a smoothing circuit, and a transfer element 7 correspond to a position feedforward term of the servo system, and symbol α1 represents a position feedforward coefficient. Transfer elements 8, 9 and 10 correspond to a speed feedforward term of the servo system; symbol Z¹ represents a Z-transform for advancing the output of the smoothing circuit 6 by a time period corresponding to one position/speed loop process; and α2 represents a speed feedforward coefficient of the servo system. The transfer elements 2, 8 and 9 are indicated by a discrete-time system. The servo system having the above-described configuration carries out the position/speed loop process a plurality of times, e.g., N times, during one distribution period in accordance with a move command periodically distributed from an upper-level control device, e.g., a numerical control device, at predetermined ITP intervals. Therefore, the servo system is supplied with move commands a(j) for position/speed loop processing periods j (= 1, 2, ..., N) included in one distribution period. Also, a move command corresponding to the distribution period subsequent to the present distribution period including each processing periods j is supplied to the servo system, for example, for use in a speed command compensation, as described later. In each position/speed loop processing period j, a feedback signal Pf supplied from the transfer element 5 as a signal representing an actual motor position is subtracted from the move command a(j) to obtain a position deviation e. Subsequently, an actual motor speed Vf is subtracted from the product of the position deviation e and the proportional gain Kp to derive a provisional speed command. On the other hand, the smoothing circuit 6 calculates a mean value of move commands based on the move command corresponding to the present distribution period and the move command corresponding to the subsequent distribution period such as a mean value of the move commands ranging from a position/speed loop processing period preceding the present processing period by (N/2) - 1 to a processing period later than the present processing period by N/2. Then, in the transfer element 7, the product of the position feedforward coefficient α1 and a transform coefficient P, which is used for transforming the move command into a speed command value, is multiplied by the mean value calculated as above to obtain a position feedforward quantity b(j), which can be expressed by the following equation (1): The position feedforward quantity b(j) is added to the provisional speed command to obtain a position feedforward-compensated speed command, and the obtained speed command is subjected to an integration at the integral term 2 of the speed loop. Then, the product of the actual motor speed Vf and the proportional constant k2 is subtracted from the output of the integral term 2 to derive a provisional torque command. On the other hand, in the transfer element 8, the output of the smoothing circuit 6 is advanced by a time period corresponding to one position/speed loop process, and in the transfer element 9, the output of the transfer element 8 is differentiated. Further, in the transfer element 10, the product of the speed feedforward coefficient α2 and a transform coefficient P' which is used to transform the move command into a current value, is multiplied by the output of the transfer element 9, whereby a speed feedforward quantity c(j) expressed in equation (2) below is obtained. Then, the speed feedforward quantity c(j) is added to the provisional torque command to obtain a speed feedforward-compensated torque command, and the servomotor is driven in accordance with the obtained torque command. Now, referring to FIG. 2, a digital servo control device constituting the servo system of FIG. 1 will be described. The digital servo control device comprises a digital servo circuit 22 including a microcomputer. A central processing unit (CPU) of the microcomputer is connected not only to a numerical control device 20 through a common memory 21 formed of a random-access memory but also to a servomotor 24 through an output circuit of the computer and a servo amplifier 23 formed of a transistor inverter, for example. Further, the CPU is connected to a pulse coder 25, which is for generating feedback pulses in accordance with the motor rotation, through an input circuit of the computer. A pulse train is generated by the pulse coder 25 at a frequency corresponding to the actual motor speed and represents the actual motor position. The CPU reads out a move command which has been stored in the common RAM 21 by the NC device 20 at the predetermined ITP intervals, and calculates move commands for N position/speed loop processing periods included in one move-command distribution period ITP, e.g., move commands for each of the four position/speed loop processing periods a(j) (j = 1,2,3,4,), in such a manner that the move commands are uniformly distributed within the period ITP. Further, the CPU executes a software processing in accordance with a control program (not shown) to accomplish the function of the servo system shown in FIG. 1. More particularly, in each position/speed loop processing period, the CPU carries out a position loop process in accordance with the present amount Pf of rotation of the motor, which corresponds to the number of the feedback pulses generated by the pulse coder 25 during the processing period concerned, and the move command a(j) to calculate the provisional speed command, and carries out a position feedforward compensation process, described in detail later, to obtain the speed command. When N = 4, i.e., when the position/speed loop process is to be executed four times within one distribution period, the CPU calculates, in the position feedforward compensation process, the position feedforward quantity b(j) in accordance with the following equation (3) corresponding to equation (1). Then, the CPU carries out the speed loop process in accordance with the present rotational speed Vf of the motor, which corresponds to the frequency of the generated feedback pulses, and the speed command to calculate a provisional torque command (current command), and carries out a speed feedforward compensation process, which will be mentioned later, to obtain the torque command. When = 2, or when the output of the smoothing circuit 6 in FIG. 1 is advanced by a time period corresponding to two position/speed loop processes, the CPU calculates the speed feedforward quantity c(j) in accordance with the following equation (4) corresponding to equation (2): Further, the CPU executes a current loop process in accordance with the torque command to obtain a PWM command. A current feedforward compensation similar to the position or speed feedforward compensation may be executed as required. The servomotor 24 is driven by the servo amplifier 23, which operates in accordance with the PWM command. Referring to FIGS. 3 and 4, the position/speed loop process, which is executed periodically, e.g., four times within one distribution period ITP by the CPU of the digital servo circuit 22, will be described in detail. At the beginning of each ITP period, the CPU reads out a move command Mc for a subsequent period ITP from the NC device 20 through the common RAM 21 (Step S100 of FIG. 3). In practice, upon start of each ITP period, the move command Mc for the present period ITP, supplied from the NC device 20, is read, whereas the position/speed loop process is executed with a time lag corresponding to one distribution period ITP, whereby an effect similar to that obtained in the case of reading out the move command Mc for the subsequent period ITP at the beginning of each ITP period can be obtained. At the beginning of a jth (j = 1, 2, 3 or 4) position/speed loop processing period in each ITP period, the CPU calculates a move command a(j + 4) for the jth position/speed loop processing period included in the subsequent ITP period on the basis of the move command Mc read in Step S100 as the move command for the ITP period subsequent to that including the present period (Step S200). Then, after transmitting the contents of registers R(j - 1) through R(j + 4) to registers R(j - 2) through R(j + 3) respectively, the calculated move command a(j + 4) is stored in the register R(j + 4) (Steps S201-1 to S201-7). Subsequently, the CPU reads out a position deviation e(j - 1) of the preceding (j - 1)th processing period from a position deviation register, and the move command a(j) for the jth processing period from the register R(j), and subtracts the present amount Pf(j) of rotation of the motor, detected by the pulse coder 25, from the sum of the position deviation e(j - 1) and the move command a(j) to obtain the position deviation e(j) of the jth processing period (Step S202). Then, the CPU calculates the position feedforward quantity b(j) in accordance with equation (3), as well as by using the move commands a(j + 1), a(j), a(j - 1) and a(j - 2) read from the corresponding registers (Step S203), obtains a provisional speed command by multiplying the position deviation e(j) by the position loop gain Kp, and adds the position feedforward quantity b(j) to the provisional speed command to obtain the speed command Vc(j) (Step S204). Further, the CPU executes the speed loop process in accordance with the speed command Vc(j), to obtain a provisional torque command Tc'(j) (Step S205). The CPU then calculates the speed feedforward quantity c(j) in accordance with equation (4), as well as by using the move command a(j + 3) read from the corresponding register (Step S206), and adds the speed feedforward quantity c(j) to the provisional torque command Tc'(j) to obtain a torque command Tc(j) (Step S207). Then, the CPU transfers the torque command Tc(j) to the current loop process (Step S208) to end the jth position/speed loop process. By way of example, FIG. 5 shows a move command Mc(n) supplied from the NC device 20 in an nth distribution period (n = 1, ..., 8), move commands a(j) calculated for the j position/speed loop processing periods (j = 1, 2, ..., 30), and values b(j)/α1·P obtained by dividing the calculated position feedforward quantity b(j) by the product α1·P. The table below shows an example of move commands a(n) for the individual position/speed loop processing periods included in the nth distribution period, the register values a(j + 4) to a(j - 2) in jth processing period and the value b(j)/α1·P. In the table, symbols +4, ..., j, ..., -2 represent the register values a(j + 4), ..., aj, ..., a(j - 2) respectively, and b(j) represents the value b(j)/α1·P. As seen from FIG. 5 and the table, even when the move command a(j) immediately changes between adjacent distribution periods ITP, the position feedforward quantity b(j) will not change rapidly unlike the conventional feedforward control using a feedforward quantity equal to the product of the differential value and the coefficient. Accordingly, the position deviation does not undergo wavy changes. In the conventional feedforward control mentioned above, the position deviation changes as shown in FIG. 6, when the move command is inputted by means of a ramp input. On the other hand, in the feedforward control according to the above-described embodiment, the position deviation changes as shown in FIG. 7 when a lead quantity 1 of the speed feedforward control is set to zero, and changes as shown in FIG. 8 when the lead quantity 1 is set to "2". As is obvious from FIGS. 6 to 8, wavy changes of the position deviation in the embodiment as much smaller than in the conventional method. In the above embodiment, when calculating the position feedforward quantity b(j) in each position/speed loop processing period, the mean value of the move commands a(j + 1), a(j), a(j - 1) and a(j - 2) is used, but the mean value of the move commands a(j + 2), a(j + 1), a(j) and a(j - 1) may alternatively be used. In this case, each position feedforward quantity b(j) in FIG.5 is equal to the value obtained by shifting it to the left by one position/speed loop processing period Ts. Further, although the above embodiment is described with reference to the case wherein the position/speed loop process is executed four times (N=4) within one distribution period ITP, the number of the position/speed loop processes to be included in one ITP period is not limited to four.

Claims: A feedforward control method for a servo system in which a position/speed loop process is periodically carried out a plural number of times within one distribution period in accordance with a move command periodically distributed from an upper-level control device, comprising the steps of: (a) executing a position loop process in each position/speed loop processing period in accordance with a corresponding move command to calculate a provisional speed command; (b) calculating a mean value of the move commands corresponding to said each position/speed loop processing period and those corresponding to position/speed loop processing periods before and after said each position/speed loop processing period; (c) calculating a feedforward controlled variable in accordance with said mean value of the move commands; and (d) correcting the provisional speed command by using said calculated feedforward controlled variable to obtain a speed command. The feedforward control method according to claim 1, wherein said step (b) includes a step of previously reading a move command for a distribution period subsequent to a distribution period including said each position/speed loop processing period. The feedforward control method according to claim 1, wherein said step (c) includes a step of multiplying the mean value of the move commands by a position feedforward coefficient. The feedforward control method according to claim 1, which further comprises calculating a provisional torque command by executing a speed loop process in said each position/speed loop processing period, subjecting said mean value of the move commands to a lead compensation by a time period equal to a predetermined times as long as the position/speed loop processing period, calculating a speed feedforward quantity in accordance with the differential value of said mean value of the move command which has been subjected to the lead compensation, and correcting the provisional torque command by using said speed feedforward quantity to obtain a torque command. The feedforward control method according to claim 4, wherein said speed feedforward quantity is calculated by multiplying a differential value of said move command mean value, which has been subjected to the lead compensation, by a speed feedforward coefficient.

Inventors: IWASHITA YASUSUKE FANUC DAI- V, IWASHITA, YASUSUKE, FANUC DAI-3 VIRA-KARAMATSU
Applicants: FANUC LTD, FANUC LTD.
Application year: 1991
Main classification: G05D3
IPC codes: G05B13, G05B19