JPH06131050A - Method for detecting collision of movable part driven by servo motor - Google Patents

Abstract

PURPOSE:To remarkably reduce errors and to accurately detect collisions with a good responsiveness by linearly approximating an estimated friction torque as linear function which is provided with the prescribed offset and is proportional to the speed. CONSTITUTION:As disturbance torques, friction torques generated by the movement of the movable part are included other than torques generated by the collision, they are estimated by reducing friction torques from the all disturbance torque estimated by the observer. In this case, as a friction torque Fu, a linearly approximated value is obtained in the following expression. In the expression Fu= Cr + k v, Cr and k are an offset value and a factor decided by each axis driven by the servo motor such as machine tools. Then, a disturbance estimation value Td2 is obtained by an all estimation disturbance value Td1, setting constant Cr, setting factor k, and real speed v, and the ratio (j/Kt) of the estimated inertia and the estimated torque constant. When the value Td2 exceeds the prescribed value, the collision is judged and the driving of the servo motor is stopped.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a machine using a servomotor such as a robot or a machine tool as a power source, in which a movable part driven by the servomotor such as a cutter of the machine tool or an arm of the robot is a work or the like. The present invention relates to a collision detection method for detecting a collision with another object.

[0002]

2. Description of the Related Art When a movable part (a cutter or a table) of a machine tool or an arm of a robot collides with an obstacle, a servomotor for driving the movable part or arm attempts to move in accordance with a movement command and produces a large torque. Occur. Therefore, the movable part and the mechanism part of the arm may be damaged. As a method of detecting this collision, etc., the position deviation of the servo system is detected, and if this position deviation exceeds the set value, a load abnormality is detected and the collision is detected. There is a method of detecting a collision assuming that a collision or the like has occurred when the current exceeds a set value. However, in the method of detecting a collision or the like by detecting an increase in the position deviation or an increase in the drive current, the servo motor is already in the state of outputting a large torque when the collision is detected. May damage the mechanical part.

Therefore, the applicant of the present application proposes a method of estimating the disturbance torque by a disturbance estimation observer and detecting that the estimated disturbance torque exceeds a set value as a load abnormality to cause a collision or the like. (Japanese Patent Laid-Open No. 3-1
96313).

Since the disturbance torque includes the friction torque caused by the movement of the movable part in addition to the torque generated by the collision, it is necessary to remove the friction torque in order to detect the collision more accurately. is there. Therefore, when a collision is detected using an observer, assuming that the friction torque is proportional to the speed, the friction torque proportional to the speed is subtracted from the total disturbance torque estimated by the observer to estimate the disturbance torque for detecting the collision. is doing.

FIG. 3 is a block diagram of a servo motor control system of a machine tool, a robot or the like, which performs proportional (P) control with respect to position and proportional and integral (PI) control with respect to speed, and shows a transfer function 10. Is a proportional gain in the position loop, transfer function 11 is a transfer function of the predistorter in the velocity loop, K1 is an integration constant, and K2 is a proportional constant. The transfer functions 12 and 13 are transfer functions of the motor, Kt is a torque constant, J is inertia, and the transfer function 14 is a transfer function that integrates the velocity v to calculate the position θ. Further, TL is the disturbance torque.

The feedback value of the current position θ is subtracted from the position command value θr, and the position deviation ε (= θr−θ) of the difference is subtracted.
Is multiplied by a proportional constant KP to obtain a speed command value, PI control is performed by PI control according to the difference (speed deviation) between the speed command value and the actual speed v, and the torque command I is calculated.
The motor is controlled by controlling the motor current based on the above. The motor rotates at the speed v, and the position θ is obtained by integrating the speed v.

FIG. 4 is a block diagram of a model of an observer object in the servo motor. Symbol 12 is a transfer function of the torque constant Kt of the servo motor shown in FIG.
b divides the transfer function 13 in FIG.
Of the transfer function 13a and the integral term 13b.
In FIG. 4, I means a torque command as an input, and v and TL mean speed and disturbance torque as state variables.

In the model of FIG. 4, the state variables v,
The equation of state for TL is as shown in Equation 1.

[0009]

[Equation 1] In Equation 1, α is acceleration, and one dot added to TL means the degree of change of the disturbance torque, but it is assumed that this value is 0 because there is no change in the disturbance torque TL in a short time. is doing.

If the same dimension observer for estimating the velocity v and the disturbance TL is formed by the general method of forming an observer from the equation shown in the equation 1, the observer indicated by reference numeral 50 in FIG. 1 is obtained. K3 and K4 of the terms 52 and 53 of the disturbance estimation observer 50 are parameters of the disturbance estimation observer,
The term 51 is the value of the parameter that multiplies the current value I as the torque command actually output to the servomotor, and is the value obtained by dividing the estimated value Kt ^* of the torque constant of the motor by the estimated value J ^* of inertia. 54 is an integral term.

The block diagram of FIG. 1 is represented by Kt = Kt ^* , J
= J ^* , {I · Kt + TL} (1 / J · S) = v (2) {I · (Kt / J) + (v−va) K3 + (v−va) (K4 / S)} (1 / S) = va (3) (where va is the estimated speed of the output of the integral term 54) From the equation (2), I = (vJS-TL) / Kt (4) ) By substituting the formula (4) into the formula (3) and rearranging, (vJS-TL) / J + (v-va) K3 + (v-va) (K4 / S) = vaS (5) S (v-va) + (v-va) .K3 + (v-va) (K4 / S) = TL / J (6) From equation (6)

[0012]

[Equation 7] From the above expression 7, the output Td1 of the term 53 is expressed by the following expression 8.

[0013]

[Equation 8] In the equation (8), if the parameters K3 and K4 are selected so that the poles are stable, it can be approximated as Td1 = TL / J, and the total disturbance torque TL can be estimated.

Then, conventionally, a value (k.multidot.k) proportional to the speed V is calculated from the total disturbance torque TL as a portion corresponding to the friction torque.
v) is subtracted, and the parameter J ^* / Kt ^{* is} added in the item 56 ^.
(J ^* is the inertia estimated value, Kt ^* is the torque constant estimated value) and the disturbance estimated value Td2 for collision detection is obtained.

[0015]

When attempting to detect a collision from a disturbance estimation value using the above-mentioned observer,
Conventionally, since the friction torque is estimated to be proportional to the speed, the estimated friction torque does not match the actual friction torque depending on the motor rotation speed, and the estimated disturbance value for detecting a collision There is a problem that Td2 has an offset and a collision cannot be detected accurately.

Therefore, an object of the present invention is to provide a collision detection method capable of detecting a collision or the like with good response and high accuracy.

[0017]

A disturbance torque received by a servo motor is estimated by using an observer. A friction torque is subtracted from the estimated disturbance torque to obtain a disturbance estimation value for collision detection. This friction torque is approximated by a linear function that has an offset and is proportional to speed. When the estimated disturbance value excluding this friction torque exceeds a predetermined value, it is determined that a collision has occurred and the servo motor is driven to stop the drive of the servo motor.

[0018]

Since the estimated friction torque is linearly approximated as a linear function having a predetermined offset and proportional to the speed, the error from the actual friction torque becomes very small. This friction torque is subtracted from the estimated value of total disturbance torque obtained by the observer.
As a result, the estimated disturbance value with the friction torque reduced represents the disturbance torque other than the friction torque.Therefore, the disturbance torque generated when the movable part driven by the servomotor collides with another object has good response, and It can be detected accurately.

[0019]

EXAMPLE The observer used in the present invention is almost the same as the conventional observer shown in FIG. 1, except that the friction torque subtracted from the total disturbance estimated value Td1 is different. Figure 2
FIG. 3 is a block diagram showing a changing part in the observer shown in FIG. 1. The only difference is that the friction torque Fu is subtracted from the total disturbance estimated value Td1 by a value that is linearly approximated by the following equation.

Fu = Cr + k · v (9) In the above equation 9, Cr and k are constants (offset values) and coefficients determined for each axis of each movable part driven by a servomotor such as a machine tool or a robot. Is. When the shaft driven by the servo motor is driven in a no-load state, the output torque of the servo motor is considered as a friction torque in a steady state of a predetermined speed. Therefore, if the output torque of the servo motor is measured at two speeds different from the steady-state speed, the constant Cr and the coefficient k are determined, and the above equation 9 is obtained.

FIG. 5 is a block diagram of a main part of a servo motor control system for carrying out the method of the present invention. Reference numeral 1 is a control device for controlling a machine tool or a robot. It is output to the digital servo circuit 3 via 2. The digital servo circuit 3 is composed of a processor, ROM, RAM, etc., digitally controls servo control of position, speed, etc., outputs a current command to a servo amplifier 4 composed of a transistor inverter, etc. It controls the motor 5. Further, 6 is a position / speed detector for detecting the position and speed, and is composed of a pulse coder or the like attached to the motor shaft of the servo motor,
A position / speed feedback signal is output to the digital servo circuit 3. Although only one axis of the servo system is shown in FIG. 5, each axis has a similar configuration, and these configurations are the same as those of a conventionally known digital servo circuit. is there.

FIG. 6 is a flowchart of a collision detection process executed by the processor of the digital servo circuit 3 every position / speed loop processing cycle. The constants K3, K4, the torque constant estimated value Kt ^* , the inertia estimated value J ^{*, the} estimated friction torque Fu constant Cr, the coefficient k, and the reference value Ts for collision detection which configure the observer are set in advance in the digital servo circuit 3. I'll do it.

The processing shown in FIG. 6 is executed for each processor position / speed loop processing cycle of the digital servo circuit 3, and first, the actual speed v of the servo motor sent from the position / speed detector 6 is read and the speed loop is executed. The torque command I obtained by the processing is read (steps S1 and S2).
Next, the estimated speed va stored in the register R (va) is subtracted from the actual speed v read in step S1 to obtain the difference Verr between the actual speed and the estimated speed (step S3). Furthermore,
A value obtained by multiplying the difference Verr by a setting constant K4 is a total disturbance estimated value T
The total disturbance estimated value Td1 in the period is calculated by adding d1 to the accumulator that stores it (step S4). That is, the process of step S4 is the process of the element 53 in FIG.

Next, a register R for storing the estimated speed va
The total disturbance estimated value Td1 obtained in step S4 is added to (va), and a constant K3 is added to the difference Verr obtained in step S3.
The value obtained by multiplying the torque command I read in the previous cycle stored in the register R (I) by the ratio (Kt ^* / J ^* ) of the estimated torque constant and the estimated inertia is added, and the value is added. The speed estimation value va of the cycle is obtained, and the register R (v
It is stored in a) (step S5). That is, the process of step S5 is a process of obtaining the estimated speed va by the process of the elements 51 and 54 in FIG.

Next, the torque command value I read in step S2 is stored in the register R (I) (step S6),
A value obtained by subtracting the estimated friction torque Fu from the total disturbance estimated value Td1 obtained in step S4 is multiplied by the ratio of the estimated inertia and the estimated torque constant (J ^* / Kt ^* ) to obtain the disturbance estimated value Td2 from which the friction torque is removed ( Step S7). That is, the following calculation is performed from the total estimated disturbance value Td1, the setting constant Cr, the setting coefficient k, and the actual speed V read in step S1, the ratio of the estimated inertia and the estimated torque constant (J ^* / Kt ^* ), and the disturbance is calculated. Obtain the estimated value Td2.

Td2 = (J ^* / Kt ^* ) (Td1-Cr-k · v) Then, the magnitude (absolute value) of the estimated disturbance value Td2 is compared with the set reference value Ts (step S8) to estimate the disturbance. Value Td2
Is less than the set reference value Ts, it is determined that no collision has occurred, and the collision detection process in the position / speed loop processing cycle is ended. However, when the magnitude of the estimated disturbance value Td2 exceeds the set reference location Ts, it is determined that a collision has occurred, the driving of the servo motor is stopped, and an alarm or the like is output (step S9).

FIG. 7 shows a disturbance estimation after the friction torque obtained by the above method is removed when a predetermined value of torque is applied as an actual disturbance to the output shaft of the servo motor with the servo motor positioned and stopped. It is the figure which compared value Td2 and the value of the actual disturbance. As can be seen from this figure, the estimated disturbance value Td2
It can be seen that follows well with the actual disturbance.

Further, FIGS. 8 to 9 show speeds 0 to 100 rp.
Disturbance estimated value Td2 of the conventional method when accelerated to m, 500 rpm, 1000 rpm (disturbance in which friction torque is removed by subtracting a value (k · v) proportional to velocity v from estimated total disturbance value Td1 shown in FIG. 1) (Estimated value Td2) (shown in FIG. 2A) and disturbance estimated value Td2 by the method of the present invention (disturbance estimated value Td2 with friction torque Fu = Cr + k · v as shown in FIG. 2). It is a figure of the experimental result which measured (shown in a figure (b)).

In the conventional method, the proportional coefficient k with respect to the speed of the friction torque is set so that the estimated disturbance value Td2 becomes "0" during steady rotation at 500 rpm. As can be seen by comparing the conventional method (a) in FIGS. 8 to 10, in FIG. 9 (a), the estimated disturbance value Td2 is almost “0” after the servomotor is accelerated to the steady rotation speed of 500 rpm. Shows a value close to. However, Figure 8 (a)
As shown in, in the case of a steady rotation speed of 100 rpm, the estimated disturbance value Td2 obtained by removing the friction torque during the steady rotation is obtained.
Does not become “0”, which is a large value. It is determined that the friction torque is large and the friction torque (k · v) is reduced by a small amount from the total disturbance estimated value Td1. FIG. 10 (a) showing the case where the steady rotation speed is 1000 rpm.
Then, the estimated disturbance value T from which the friction torque is removed during steady rotation
It can be seen that the value of d2 is negative and the friction torque is reduced too much. Also, it can be seen that the overshoot largely appears during acceleration.

On the other hand, in the method of the present invention shown in (b) of FIGS. 8 to 10, the disturbance estimated value Td2 from which the friction torque is removed is a value close to "0" at any rotation speed during steady rotation. It can be seen that the friction torque is accurately removed. Also, no large overshoot occurs during acceleration. From this, if the disturbance estimated value Td2 from which the friction torque is removed is obtained by the method of the present invention, the disturbance estimated value Td2 can be obtained without being affected by the friction torque regardless of the number of revolutions, and Since a large overshoot does not occur even during acceleration or the like, the reference value Ts for detecting a collision is set to a smaller value as compared with the conventional method, and more accurately, and when a collision occurs, the servo is used at an earlier stage. It is possible to detect that the movable portion driven by the motor has collided with another object.

[0031]

According to the present invention, the estimated disturbance value after the friction torque is removed more accurately can be obtained. Therefore, the accuracy can be improved by setting the reference value for detecting a collision to a small value. Because you can do it faster,
And it can be detected accurately.

[Brief description of drawings]

FIG. 1 is a block diagram of a conventional observer that obtains a disturbance estimated value from which friction torque is removed.

FIG. 2 is a block diagram of a main part of an observer used in the present invention.

FIG. 3 is a block diagram of a servo motor control system.

FIG. 4 is a block diagram of a model that forms a disturbance estimation observer.

FIG. 5 is a block diagram of a digital servo system that implements an embodiment of the present invention.

FIG. 6 is a flowchart of a collision detection process of one embodiment of the present invention executed by a processor of a digital servo circuit.

FIG. 7 is a diagram showing a comparison between a disturbance estimated value and an actual torque obtained by removing a friction torque obtained when a predetermined value of the actual torque is applied to the shaft of the servo motor in the method of the present invention. Is.

FIG. 8: The speed of the servo motor is changed from “0” to 100 rpm
It is the figure which calculated | required the disturbance estimated value which removed the friction torque by the conventional method and the method of this invention at the time of accelerating to, and made it a graph.

[Fig. 9] Servo motor speed is changed from "0" to 500 rpm.
It is the figure which calculated | required the disturbance estimated value which removed the friction torque by the conventional method and the method of this invention at the time of accelerating to, and made it a graph.

FIG. 10: The servo motor speed is changed from "0" to 1000r.
It is the figure which calculated | required the disturbance estimated value which removed the friction torque by the conventional method at the time of accelerating to pm, and the method of this invention, and was made into the graph.

[Explanation of symbols]

 1 Controller 2 Shared memory 3 Digital servo circuit 4 Servo amplifier 5 Servo motor 6 Position / speed detector 50 Disturbance estimation observer TL Disturbance torque Td1 Total disturbance estimated value Td2 Disturbance estimated value Ts for removing friction torque For collision detection Standard value

Claims (1)

[Claims] 1. A disturbance torque received by a servomotor is estimated by using an observer, and from the estimated disturbance torque,
A friction torque approximated by a linear function as a value proportional to speed with an offset is subtracted to obtain an estimated disturbance value that excludes the friction torque. When the estimated disturbance value exceeds a predetermined value, it is determined that a collision occurs and the servo motor Of collision detection of a movable part driven by a servo motor that stops driving of the.