Method of controlling a servomotor

Abstract

A servomotor control method utilizing conventional linear control techniques, which permits easy introduction of sliding mode control. The phase surface of the sliding mode is made to be equivalent to a mode of obtaining a torque command by a conventional linear control, and a switching input (.tau.1) is added to the torque command (.tau.0) obtained by the linear control, thereby further obtaining a corrected torque command (.tau.). The switching input (.tau.1) is determined in accordance with the positive or negative sign of the torque command (.tau.0) and the signs of various values such as a position deviation (.epsilon.), a speed loop integral, etc., which are obtained during the calculation of the torque command (.tau.0).

Description

BACKGROUND OF THE INVENTION

1. Field of Invention

This invention relates to a method of controlling a servomotor for driving various machines such as a robot and a machine tool, and more particularly, to a sliding mode control.

2. Description of the Related Art

A variety of sliding mode control techniques are used for controlling servomotors in various machines to improve disturbance suppressing ability and achieve good follow-up with respect to commands. Such sliding mode control techniques are disclosed, for example, in Unexamined Japanese Patent Publications No. 2-297611 and No. 3-118618.

These conventional sliding mode control techniques, however, have no relevance to P control (proportional control) for a position loop or to PI control (proportional-plus-integral control) for a speed loop, which have conventionally been employed for controlling servomotors. Thus, conventional linear control techniques elaborated for the P and PI controls cannot be utilized at all for the control of a servomotor.

Thus, the application of the sliding mode control has been hindered mainly due to that the selection of various parameters is so time consuming and some other reasons.

SUMMARY OF THE INVENTION

The object of this invention is to permit easy introduction of sliding mode control, and to utilize conventional linear control techniques.

According to this invention, the phase surface of the sliding mode is set to be in proportion to a mode of obtaining a torque command when carrying out a proportional control for a position loop and a proportional-plus-integral control for a speed loop, and a switching input is added to the torque command obtained by a linear control process including the position loop process according to the proportional control and the speed loop process according to the proportional-plus-integral control, to obtain a corrected torque command. The servomotor is controlled in accordance with the corrected torque command.

The phase surface of the sliding mode is made to be equivalent to the mode of obtaining a torque command by a linear control in which the position loop is subjected to proportional control and the speed loop is subjected to proportional-plus-integral control. Thus, by adding the switching input to the torque command obtained by the linear control process, a torque command for the current loop is obtained, and the sliding mode control can be easily applied.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram illustrating a servomotor control method according to one embodiment of this invention;

FIG. 2 is a block diagram of a principal part of a servomotor control unit for carrying out the control method illustrated in FIG. 1;

FIG. 3 is part of a flowchart showing a feedforward process, position loop process, speed loop process, and sliding mode process, which are executed by a processor of a digital servo circuit shown in FIG. 2; and

FIG. 4 and FIG. 5 are individually the subsequent part of the flowchart shown in FIG. 3.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

First, a phase surface (switching surface) Suf is defined as expressed by the following equation (1):

Suf=.epsilon.+Kp.multidot..epsilon.+(K1/K2).multidot..intg.(.epsilon.+Kp.multidot..epsilon.) (1)

In equation (1), Kp is a position gain of a position loop according to P control, and K1 and K2 are an integral gain and a proportional gain, respectively, of a speed loop according to PI control. .epsilon. represents a position deviation, while .epsilon. represents the derivative of the position deviation, i.e., a speed deviation. In the following, symbols with one dot give above denote first-time derivatives, and symbols with two dots given above denote second-time derivatives.

For the Liapunov function V, the following formula (2) is defined:

V=(1/2).multidot.Suf.sup.2 (2)

The derivative of the Liapunov function V is given by the following equation (3):

V=Suf.multidot.Suf (3)

If a servomotor is supplied with such an input that the resulting value of equation (30 is always negative, then the Liapunov function V is always positive, and the derivative thereof is negative, showing a monotonic decrease, so that the Liapunov function V and the phase surface Suf coverage to the minimum value "0." Accordingly, the responsiveness converges to Suf=0 and varies functionally, without being influenced by inertia or disturbance.

Here, the input toe the servomotor is defined as expressed by the following equation (4): ##EQU1##

In equation (4), .tau.1 is a switching input.

Given that a move command and a position feedback quantity are .eta.d and .eta., respectively, the following equations (5) hold:

.epsilon.=.eta.d-.eta. .epsilon.=.eta.d-.eta. .epsilon.=.eta.d-.eta. (5)

FIG. 1 shows a block diagram for obtaining the servomotor input .tau. in equation (4) on the basis of equations (5). As seen from FIG. 1, the servomotor input .tau. is equal to the sum of the switching input .tau.1 and an output (torque command .tau.0) of a conventional linear control in which the position and speed loops are subjected to P control and PI control, respectively. In FIG. 1, Kt/(J.multidot.S) is the transfer function of the servomotor; Kt is the torque constant of the servomotor; J is the inertia; and S is the Laplace operator.

It is assumed that the controlled system is given by equation (6) below.

J.multidot..eta.+A.multidot..eta.+Gr+.tau.d=.tau. (8)

In equation (6), A is the coefficient of kinetic friction; Gr is a gravity term, and .tau.d is an expected disturbance.

Differentiating both sides of equation (1) provides equation (7).

Suf=.epsilon.+{Kp+(K1/K2)}.multidot..epsilon.+(K1/K2).multidot.Kp.multidot..epsilon. (7)

Substituting equations (5) for equation (6), followed by rearrangement, provides the following equation (8):

.epsilon.=.eta.d+(A/J).multidot..eta.+(Gr/J)+(.tau.d/J)-(.tau./J)(8)

Then, when equation (4) is substituted for equation (8), equation (9) below is obtained: ##EQU2##

Substituting equation (9) for equation (7), followed by rearrangement, provides equation (10). ##EQU3##

Then, when equation (1) is solved for the derivative term of the position deviation, the following equation (11) is obtained:

.epsilon.=Suf-Kp.multidot..epsilon.-(K1/K2).multidot..intg.(.epsilon.+Kp.multidot..epsilon.) (11)

Substituting equation (11) for the equation (10), followed by rearrangement, provides the following equation (12): ##EQU4##

From equation (12), the derivative of the Liapunov function is given as follows: ##EQU5##

Consequently, in order for the derivative of the Liapunov function to be always negative, the following formulas (14) and (15) need to hold: ##EQU6##

Given that the variation range of the inertia J of the controlled system is Jmin.ltoreq.J.ltoreq.Jmax (Jmin and Jmax are expected minimum and maximum inertias, respectively), in order for formula (14') to always hold, the value of Kp must be smaller than the minimum value of the right side. Accordingly, the position gain Kp of the position loop and the integral and proportional gains K1 and K2 of the speed loop need be selected in order for the following formula (16) to hold:

Kp<(K2/Jmax)-(K1/K2) (16)

Here, it is assumed that the switching input .tau.1 can be expressed using terms related to .epsilon., .intg.(.epsilon.+Kp.multidot..epsilon.), .eta.d, .eta., Gr and .tau.d, as shown in the following equation (17):

.tau.1=L1(.epsilon.)+L2{.intg.(.epsilon.+Kp.multidot..epsilon.)}+L3(.theta.d)+L4(.theta.)+L5(Gr)+L6(.tau.d) (17)

From equation (17), formula (15) can be rewritten into the following formula (18): ##EQU7##

In order for formula (18) to hold at all times, the terms related to .epsilon., .intg.(.epsilon.+Kp.multidot..epsilon.), .theta.d, .theta., Gr and .tau.d must individually be negative. That is , the elements L1(.epsilon.), L2(.intg.(.epsilon.+Kp.multidot..epsilon.)), L3(.eta.d), L4(.theta.), L5(Gr) and L6(.tau.d) of the switching input need to be determined as follows:

(1) When Suf.gtoreq.0, I) for L1(.epsilon.),

Kp.sup.2 .multidot..epsilon.={L1(.epsilon.)/J}>0 need to hold, i.e., L1(.epsilon.)>-Kp.sup.2 .multidot.J.multidot..epsilon., and, for this formula to hold at all times in the range Jmin.ltoreq.J.ltoreq.Jmax, the following conditions need be fulfilled:

if .epsilon..gtoreq.0, L1(.epsilon.)=-Kp.sup.2 .multidot.Jmin.multidot..epsilon. if .epsilon.<0, L1(.epsilon.)=-Kp.sup.2 .multidot.Jmax.multidot..epsilon.(19)

The individual elements of the switching input are determined in a single manner. ##EQU8## (2) When Suf<0, ##EQU9##

In the above formulas (19) to (30), Jmax and Jmin are expected maximum and minimum inertias, respectively, of the controlled system; Amax and Amin, expected maximum and minimum coefficients of kinetic friction, respectively; Grmax and Grmin, expected maximum and minimum gravities, respectively; and .tau.dmax and .tau.dmin, expected maximum and minimum disturbances, respectively.

The switching input .tau.1 is determined in the aforementioned manner, and is added to the torque command obtained by ordinary linear control (involving position loop control based on P control and speed loop control based on PI control) so that the derivatives of the Liapunov function can always be kept negative, whereby a stable control system converging to the switching surface can be obtained.

As seen from equation (1) and FIG. 1, the value of the switching surface Suf can be obtained by dividing the torque command .tau.0, obtained by the linear control, by the proportional gain K2 of the speed loop. Thus, the sign, positive or negative, of the switching surface Suf is dependent on the sign of the torque command value obtained by the linear control, and parameters determining the values of the individual terms of the switching input .tau.1 are derived during the linear control processing.

FIG. 2 is a block diagram of a principal part of a servomotor control unit for carrying out the method of this invention. Referring to FIG. 2, a common memory 21 receives various commands for a servomotor 24, from a host CPU 20, and transfers them to the processor of a digital servo circuit 22. The digital servo circuit 22 comprises, in addition to the processing, a ROM, a RAM, etc., and the processor executes processing for controlling the position, speed, and current of the servomotor 24. A servo amplifier 23 comprises a transistor inverter or the like, and drives the servomotor 24 in accordance with the signal from the digital servo circuit 22. A position/speed detector 25 detects the rotational position and speed of the servomotor 24, and feeds back signals representing the detected position and speed to the digital servo circuit 22.

The above-described configuration is similar to that of a well-known digital servo circuit used for controlling servomotors in a robot, machine tool or the like.

FIGS. 3 to 5 illustrate a flowchart for a feedforward processing, position loop processing, speed loop processing, and sliding mode processing which are executed by the processor of the digital servo circuit 22 in this embodiment.

The processor executes the processing shown in FIGS. 3 to 5 at predetermined intervals of time (position/speed loop processing period).

First, in Step S1, a move command .eta.d for each present position/speed loop is obtained based on the move command supplied from the host CPU 20 via the common memory 21, and the outputs of position and speed feedback values from the position/speed detector 25 are read. Then, in Step S2, a value obtained by subtracting the position feedback value .theta. from the move command .theta.d is added to the value in a register R(.epsilon.), which stores the position deviation .epsilon., to thereby obtain a position deviation .epsilon. of the relevant cycle.

Subsequently, in Step S3, the value in a register R(.theta.d), which stores the move command of the preceding cycle, is subtracted from the move command .theta.d to obtain a feedforward quantity .theta.d (speed of the move command .theta.d) (differential of the move command in FIG. 1). Then, in Step S4, the value in a register, which stores the feedforward quantity of the preceding cycle, is subtracted from the feedforward quantity .theta.d to obtain an acceleration .theta.d of the move command. In the following Step S5, the move command .theta.d obtained in Step S1 and the feedforward quantity .theta.d obtained in Step S3 are stored in the register for storing the move command and the register for storing the feedforward quantity respectively for use in the subsequent cycle.

Then, in Step S6, a value Vd is obtained by adding the feedforward quantity .theta.d to the product of the position gain Kp and the position deviation .epsilon., which has been obtained in Step S2, and then by subtracting the speed feedback value .theta., which has been read in Step S1, from the previously obtained summation value. In Step S7, the value Vd is added to the value in an accumulator I to obtain an integral value (integration in the speed loop process shown in FIG. 1). In Step S8, the product of the value in the accumulator I and the integral gain K1 is added to the product of the value Vd and the proportional gain K2, to obtain a torque command .tau.0 according to linear control (processing according to the position loop P control and speed loop PI control using conventional feedforward).

The position gain Kp, the integral gain K1, and the proportional gain K2 are set in advances as parameters to satisfy the relationship expressed by formula (16), and the maximum and minimum values such as Jmax and Jmin of inertia, the maximum and minimum values Amax and Amin of kinetic friction coefficient, the maximum and minimum values Grmax and Grmin of the gravity term, and the expected maximum and minimum values .tau.dmax and .tau.dmin of disturbance are also set as parameters. These parameters are stored in a memory of the digital servo circuit.

In Step S9, it is judged whether the torque command .tau.0 has a value greater than or equal to "0." The sign of the torque command .tau.0 is identical with that of the switching surface Suf as mentioned above, and thus the sign of the switching surface Suf can be judged in accordance with the sign of the torque command .tau.0. If the torque command .tau.0 is "0"0 or a positive value, the individual terms of the switching input .tau.1 are determined the following manner, as shown in formulas (19) to (24).

First, if the position deviation .epsilon., obtained in Step S2, has a value greater than or equal to "0," "-Kp.sup.2 .multidot.Jmin.multidot..epsilon." will be stored in a register L1, and, if the position deviation .epsilon. is negative, "-Kp.sup.2 .multidot.Jmax.multidot..epsilon." will be stored in the register L1 (Steps S10 to S12). That is, the processing represented by formulas (19) will be executed.

Subsequently, if the integral value obtained in Step S7 and stored in the accumulator I has a value greater than or equal to "0," "-Jmin.multidot.{Kp+(K1/K2)}.multidot.(K1/K2).multidot.(value of the accumulator I)" is stored in a register L2, and, if the integral value stored in the accumulator I is negative, "-Jmax.multidot.{Kp+(K1/K2)}.multidot.(K1/K2).multidot.(value of the accumulator I)" is stored in the register L2 (Steps S13 to S15). That is, the processing represented by formulas (20) is carried out.

Further, in Steps S16 to S18, it is judged whether the acceleration obtained in Step S4, which is the second-time derivative of the move command, has a value greater than or equal to "0," and if the value of the acceleration is "0" or more, the product of the acceleration value and the maximum inertia value Jmax is stored in a register L3; if the acceleration value is negative, the product of the acceleration value and the minimum inertia value Jmin is stored in the register L3. This processing is equivalent to that represented by formulas (21).

Then, in Steps S19 to S21, it is judged whether the speed feedback has a value greater than or equal to "0," and, if the speed feedback value is "0" or more, the product of the speed feedback value and the maximum friction coefficient Amax is stored in a register L4; if the speed feedback value is negative, the product of the speed feedback value and the minimum friction coefficient Amin is stored in the register L4. This processing is equivalent to that represented by formula (22).

Further, in Step S22, "Grmax" is stored in a register L5, and, in Step S23, ".tau.dmax" is stored in a register L6 (processing expressed by formulas (28) and (29)).

In Step S24, the values stored in the individual registers L1 to L6 are added together to obtain a switching input .tau.1, and then, in Step S25, the thus-obtained switching input .tau.1 is added to the torque command .tau.0, which is obtained in Step S8 by the linear control processing, thereby obtaining a corrected torque command .tau.. The torque command .tau. is outputted to a current looping Step S26, and the processing for the relevant position/speed loop processing period is ended.

If it is judged in Step S9 that the torque command .tau.0 based on the linear control is negative, the program proceeds from Step S9 to Step S27. In the subsequent Steps S27 to S40, the judgment and processing represented by formulas (25) to (30) are executed to obtain values of the individual terms of the switching input .tau.1, and the values obtained are stored in the registers L1 to L6. After executing Step S40, the program returns to Step S24, and, in Steps S24 to S26, the values stored in the registers L1 to L6 are added together to obtain a switching input .tau.1; the switching input .tau.1 is added to the torque command .tau.0, which is obtained by the linear control process, to obtain a torque command .tau.; and the obtained torque command is outputted to the current loop, as described above.

The aforementioned processing is executed for each position/speed loop processing period.

Now, how to execute the bang-bang control (method of controlling a servomotor only by means of a .+-.maximum current thereof by adjusting the driving current to .+-.maximum current) will be described.

In this case, the switching input .tau.1 is set to a value equal to the absolute value of a maximum torque .tau.max that the servomotor can output, with a sing (positive or negative) thereof set to that identical with the sing of the switching surface Suf, i.e., the sign of the output .tau.0 of the linear control. This procedure can remarkably simplify the processing. More specifically, in Step S9, the sign of .tau.0 is read instead of executing Steps S10 to S40, the read sign is attached to the maximum torque .tau.max to obtain a switching input .tau.1, and the output .tau.0 of the linear control is added to the switching input .tau.1, with the result to be used as a torque command .tau. for the current loop.

In the above embodiment, feedforward control is used for the linear control, but this invention can be applied to an ordinary control not based on feedforward control such as a P control for position control and a PI control for speed control. In such cases, when obtaining the value Vd, which is inputted to the speed loop for processing the integral and proportional terms, the value (.epsilon.) obtained by differentiating the position deviation .epsilon. is replaced by the negative speed feedback value (-.theta.). That is, an ordinary speed deviation is used for the value Vd. Thus, the term of function L3 is omitted, switching input also varies correspondingly.

According to this invention, sliding mode control can easily be applied without substantially modifying conventional linear control, whereby the disturbance suppressing ability can be improved. Furthermore, since conventional linear control is sued, various linear control techniques developed so far can be utilized.

Claims

1. A method of controlling a servomotor in a sliding mode, comprising the steps of:

obtaining a torque command (.tau.0) for carrying out a proportional control for a position loop and a proportional-plus-integral control for a speed loop;

setting a phase surface of the sliding mode to be in proportion to the torque command (.tau.0) obtained by a position loop processing according to the proportional control and a speed loop processing according to the proportional-plus-integral control;

providing individual gains for a switching input (.tau.1) which are set based on the following relationship:

Kp<(K2/Jmax)-(K1/K2)

where Kp is a position gain of the position loop, K1 is an integral gain of the speed loop, K2 is a proportional gain of the speed loop, and Jmax is a maximum inertia of a controlled system, so that the derivative of a Liapunov function is always negative, and is determined in accordance with signs of respective values of the torque command (.tau.0), a position deviation (.epsilon.), an integral (I) of the position deviation and a speed deviation, acceleration (.theta.d) of a move command, and speed feedback (.theta.), which are obtained by the position loop process according to the proportional control and the speed loop process according to the proportional-plus-integral control;

adding the switching input (.tau.1) to the torque command (.tau.0) to obtain a corrected torque command (.tau.); and

driving the servomotor in accordance with the corrected torque command (.tau.).

2. A method according to claim 1, wherein, the controlled system is expressed as

J.multidot..theta.+A.multidot..theta.+Gr+.tau.d=.tau.

where J is an inertia, A is a coefficient of kinetic friction, Gr is a gravity term, and .tau.d is an expected disturbance, and that the switching input (.tau.1) is expressed as

.tau.1=L1(.epsilon.)+L2{.intg.(.epsilon.+Kp.multidot..epsilon.)}+L3(.theta.d)+L4(.theta.)+L5(Gr)+L6(.tau.d),

respective values of elements L1(.epsilon.), L2{.intg.(.epsilon.+Kp.multidot..epsilon.)}, L3(.theta.d), L4(.theta.), L5(Gr) and L6(.tau.d) of the switching input are set such that a relationship of ##EQU10## is satisfied.

3. A method of controlling a servomotor in a sliding mode, comprising the steps of:

obtaining a torque command (.tau.0) for carrying out a proportional control for a position loop and a proportional-plus-integral control for a speed loop;

setting a phase surface of the sliding mode to be in proportion to the torque command (.tau.0) obtained by a position loop processing according to the proportional control and a speed loop processing according to the proportional-plus-integral control;

providing individual gains for a switching input (.tau.1) which are set based on the following relationship;

Kp<(K2/Jmax)-(K1/K2)

where Kp is a position gain of the position loop, K1 is an integral gain of the speed loop, K2 is a proportional gain of the speed loop, and Jmax is a maximum inertia of a controlled system, and is given as a value which is a combination of an absolute value of a maximum torque (.tau.max) that the servomotor can output, and a positive or negative sign identical with that of the torque command (.tau.0), which is obtained by the position loop process according to the proportional control and the speed loop process according to the proportional-plus-integral control;

adding the switching input (.tau.1) to the torque command (.tau.0) to obtain a corrected torque command (.tau.); and

driving the servomotor in accordance with the corrected torque command (.tau.).