JP2020010447A - Electric motor control device and collision detection method - Google Patents

Abstract

To provide an electric motor control device and a collision detection method in which the accuracy of detecting collisions is improved by taking an electric motor speed into consideration in determining a collision of a load machine.SOLUTION: The electric motor control device includes a disturbance observer 9 that estimates a disturbance torque which acts on an electric motor 2 for driving a load machine 1 from the load machine 1 by using a torque command value and a speed of the electric motor 2, a differentiator 10 that calculates the time change of the estimated disturbance torque obtained by the disturbance observer 9, a collision detector 11 that detects a collision of the load machine 1 with an obstacle on the basis of an output from the differentiator 10, and a collision controller 12 and a speed controller 7 for controlling the speed of the electric motor 2 to a safety side at a time of detection of the collision. The collision detector 11 changes a threshold for detecting collisions such that the threshold is proportional to the speed within a fixed speed range of the electric motor 2.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a motor control device having a collision detection function when an object driven by a motor collides with an obstacle, and a collision detection method.

2. Description of the Related Art In a control device for an electric motor that drives a machine tool, a robot arm, or the like, there is a related art that detects that a driving target collides with a surrounding obstacle.
For example, Patent Literature 1 discloses a servo motor position control device shown in FIG. 8, the position control unit 102 calculates the speed command value V _C so that the position detection value P _f by the position detector 107 follows the position command value P _C from the position command generator 101, the speed control unit 103, speed detection value _{V f} from the speed detecting means 108 computes a current command value _{I C} so as to follow the speed command value _{V C.}

A reverse rotation brake control unit 111 includes a current command value I _C and the velocity detection value V _f, the braking torque command value, a collision determination signal from the determination unit 110 to be described later is input. The plugging control unit 111, during normal operation and outputs the current command value I _C, at the time of input of a collision determination signal, and calculates a current command value I _S for stopping the motor 105 in accordance with the braking torque command value output I do.
The current control unit 104 controls the motor 105 in accordance with the current command value _{I C} or _{I S,} driving or stopping the machine unit 106 is a driving object.

Here, the disturbance observer 109 estimates the disturbance torque acting on the mechanical part 106 based on the speed detection value V _f and the current command value I _C, and outputs the estimated disturbance torque tau _L ^# to the determination unit 110. When the absolute value of the estimated disturbance torque τ _L ^# exceeds the allowable disturbance torque τ _La from the operation mode (positioning operation mode or cutting feed mode) determination unit 112, the mechanical unit 106 collides with the obstacle. It is determined that a collision has occurred, and a collision determination signal to the reverse rotation braking control unit 111 and an alarm to the outside are output.
A control device that stops a servomotor when an abnormal load is applied to a robot arm based on the same principle as this conventional technology is also described in Patent Document 2.

FIG. 9 is a waveform chart showing the operation of the position control device shown in FIG.
9 (a), the case where the disturbance observer 109 is equal to the actual value of inertia moment value J is used for estimating calculation of the disturbance torque, the mechanical portion 106 collides with an obstacle at the time t _c, the disturbance observer 109 The estimated disturbance torque τ _L ^# is appropriately calculated, and the collision of the mechanical unit 106 can be detected because the estimated disturbance torque τ _L ^# exceeds the allowable disturbance torque τ _La .

However, when the moment of inertia J used for the estimation calculation is, for example, ten times the actual value, as shown in FIG. 9B, a calculation error due to the torque component at the time of acceleration / deceleration of the motor causes an estimated disturbance torque τ. to appear in _L ^#, which may result in erroneous detection of collision when small allowable disturbance torque tau _La.
Thus, in the prior art of Patent Document 1, it is difficult to increase the collision detection sensitivity is set smaller the allowable disturbance torque tau _La.

On the other hand, Patent Literature 3 discloses a collision detection device in which an error included in the estimated disturbance torque is reduced to increase the collision detection sensitivity.
FIG. 10 is a block diagram of the collision detection device. The control unit 202 controls the servomotor 200 that drives the target 201, and the disturbance observer 203 calculates the disturbance torque acting on the target 201 based on the speed detection value of the servomotor 200 and the torque command value of the control unit 202. It is estimated as the first disturbance torque τ _calc1 .

The first estimated disturbance torque τ _calc1 is input to a filter 204 including a high-pass filter or the like, and is input to the collision determination unit 205 as a second estimated disturbance torque τ _calc2 from which low-frequency components have been removed. The collision determination unit 205 determines the collision of the object 201 when the second estimated disturbance torque τ _calc2 exceeds the sensitivity setting value limit, and outputs a determination such as an alarm.

In this prior art, the estimated disturbance torque by the disturbance observer 203 includes an error of a parameter such as a moment of inertia used in the estimation calculation, a calculation error caused by gravity and frictional force, and a frequency component of the error is a disturbance torque due to a collision. It is noted that the frequency component is sufficiently lower than the frequency component.
That is, when a calculation error τ _err between the actual value τ _real of the disturbance torque and the estimated disturbance torque τ _calc shown in FIG. 11A is considered, the filter 204 removes the frequency component corresponding to τ _err. Therefore, there is no need to set τ _err to a relatively large value (resulting in increasing the sensitivity setting value limit) as shown in FIG. 11B, and to set the sensitivity setting value limit to τ _err ≒ 0 as shown in FIG. 11C. Can be reduced.

Japanese Patent Application Laid-Open No. 6-284774 ([0022] to [0043], FIG. 1 and the like) JP-A-2-30487 (FIG. 1, etc.) JP-A-2001-51721 ([0005] to [0010], FIG. 3, FIG. 5, etc.)

As described above, in the collision detection device of Patent Document 3, the calculation error τ _err at the time of disturbance torque estimation is reduced to almost zero by extracting the frequency component caused by the collision, and the sensitivity setting value limit can be set to a small value. Is being improved.
However, the speed of the servomotor 200 is high, and the disturbance torque (collision force) when the object 201 driven at high speed collides is larger than that at low speed. Therefore, if the sensitivity setting value limit used for collision determination is fixed, it may not be possible to accurately detect a collision depending on the motor speed, so that further improvement in collision detection accuracy has been demanded.

  Therefore, an object of the present invention is to provide a motor control device and a collision detection method that further improve the collision detection accuracy by reflecting the motor speed in the collision determination.

In order to solve the above problem, an electric motor control device according to claim 1 converts a disturbance torque acting on the electric motor when a load machine driven by the electric motor collides with an obstacle into a torque command value and a speed of the electric motor. Disturbance torque estimating means for estimating based on the
Differentiating means for calculating a temporal change of the estimated disturbance torque output from the disturbance torque estimation means,
Collision detection means for detecting that the load machine has collided with an obstacle based on the output of the differentiating means,
Control means for controlling the speed of the electric motor to a safe side at the time of collision detection by the collision detection means,
In the motor control device provided with
Means for generating a threshold value for collision detection that changes according to the speed of the electric motor,
The collision detecting means detects a collision of the load machine when an output of the differentiating means exceeds the threshold value.

  An electric motor control device according to a second aspect is the electric motor control device according to the first aspect, wherein the threshold value is changed so as to be proportional to the speed in a constant speed range of the electric motor.

According to a third aspect of the present invention, in the electric motor control device according to the first or second aspect, the threshold value is changed according to a detected speed value of the electric motor.
According to a fourth aspect of the present invention, in the electric motor control device according to the first or second aspect, the threshold value is changed according to a speed command value of the electric motor.

  A motor control device according to a fifth aspect of the present invention is the motor control device according to any one of the first to fourth aspects, wherein the differentiating means includes a filter such as a high-pass filter.

The collision detection method according to claim 6 estimates and estimates a disturbance torque acting on the motor when a load machine driven by the motor collides with an obstacle based on a torque command value and a speed of the motor. In a collision detection method for detecting that the load machine has collided with an obstacle based on a differential value of a disturbance torque,
A threshold value for collision detection to be compared with the differential value is changed according to the speed of the electric motor, and when the differential value exceeds the threshold value, a collision of the load machine is detected.

The electric motor control device according to claim 7, wherein a collision force acting on the electric motor when a load machine driven by the electric motor collides with an obstacle is estimated based on a displacement of the load machine and a spring constant of the obstacle. Collision force estimating means,
Means for extracting a predetermined frequency component of the estimated collision force output from the collision force estimation means,
Collision detection means for detecting that the load machine has collided with an obstacle based on the frequency component,
Control means for controlling the speed of the electric motor to a safe side at the time of collision detection by the collision detection means,
In the motor control device provided with
Means for generating a threshold value for collision detection that changes according to the speed of the electric motor,
The collision detection means detects a collision of the load machine when the frequency component exceeds the threshold.

A collision detection method according to claim 8, wherein a collision force acting on the electric motor when a load machine driven by the electric motor collides with an obstacle is estimated based on a displacement of the load machine and a spring constant of the obstacle. A collision detection method for detecting that the load machine has collided with an obstacle based on a predetermined frequency component of the estimated collision force,
A threshold for collision detection to be compared with the frequency component is changed according to the speed of the electric motor, and a collision of the load machine is detected when the frequency component exceeds the threshold. .

According to the first to sixth aspects of the present invention, by detecting the collision by comparing the threshold value changed according to the motor speed with the differential value of the estimated disturbance torque, even when the motor speed changes over a wide range. Thus, it is possible to realize a motor control device and a collision detection method that are robust against estimation errors. In particular, by increasing the threshold value for collision detection as the motor speed increases, the collision detection level can be stabilized.
According to the seventh and eighth aspects of the present invention, a collision can be detected with high accuracy even when the colliding obstacle is an elastic body.

It is a block diagram of a 1st embodiment of the present invention. FIG. 2 is a block diagram illustrating a configuration of a disturbance observer in FIG. 1. FIG. 4 is a characteristic diagram of a threshold according to the first embodiment of the present invention. FIG. 4 is a waveform chart showing an operation at the time of a collision in the first embodiment of the present invention. FIG. 4 is a waveform chart showing an operation at the time of a collision in the first embodiment of the present invention. FIG. 9 is a block diagram illustrating a main part of a second embodiment of the present invention. FIG. 13 is a block diagram illustrating a main part of a third embodiment of the present invention. FIG. 10 is a block diagram of a position control device described in Patent Document 1. FIG. 9 is a waveform chart showing the operation of FIG. 8. FIG. 11 is a block diagram of a collision detection device described in Patent Literature 3. FIG. 11 is a waveform chart showing the operation of FIG. 10.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 is a block diagram of a motor control device having a collision detection function according to a first embodiment of the present invention corresponding to claims 1 to 6. In FIG. 1, reference numeral 1 denotes a load machine (drive target) driven by an electric motor 2 such as a servomotor, 3 a position detector for detecting the position of a rotor of the electric motor 2, and 4 a speed based on a detected position value. It is a speed detector.

5 is a position command generator for sequentially outputting the position command value of the electric motor 2, 6 is a position controller for calculating a speed command value so that the fed back position detection value follows the position command value, and 7 is A speed controller 8 calculates a torque command value τ _r so that a detected speed value of the motor 2 follows the speed command value, and a current controller 8 controls the current of the motor 2 according to the torque command value τ _r .

Further, 9 is a disturbance observer for estimating a disturbance torque acting on the load machine 1 and the electric motor 2 from the speed detection value _nm and the torque command value τ _r of the electric motor 2, and 10 is a differential for approximating the estimated disturbance torque τ _d ^#. It is a vessel. Also, 11 is the estimated disturbance torque differential value tau _{d #} ^'output from the differentiator 10, the threshold (the initial value) tau _th0 input from the outside, to the speed detection value n _m of the electric motor 2 based on the collision detector detects a collision of the load machine 1, 12, load machine 1 and the motor 2 in accordance with the collision detection signal S _d from the collision detector 11, a speed command value to avoid breakage of the obstacle This is a collision controller that generates and controls the speed of the electric motor 2 on the safe side.

Next, the configuration of the disturbance observer 9 will be described with reference to FIG.
As shown in FIG. 2, the disturbance observer 9 subtracts the result obtained by multiplying the total moment of inertia value J _n of the nominal by differentiating the speed detection value n _m of the motor 2 from the torque command value tau _r, the time constant T The estimated disturbance torque τ _d ^# is calculated through a low-pass filter. Note that s is a differential operator.

FIG. 3 is a characteristic diagram of the threshold value τ _th compared with the estimated disturbance torque differential value τ _d ^# ′ in the collision detector 11. The threshold tau _th is the threshold tau _th0 externally given as an initial value, ranging up to the speed detection value n _m reaches n ₁ is in proportion to the speed, the speed detection value n _m is the n ₁ If it exceeds, it is weighted to be constant.

Operation of normal in this embodiment (at the time of non-collision) is substantially the same as that of the prior art, such as FIG. 8, the speed controller 7 such that the speed detection value n _m of the electric motor 2 to follow the speed command value and the torque command A value is generated, and the current controller 8 controls the electric motor 2 according to the torque command value to drive the load machine 1 to the target position.

Next, an operation when the load machine 1 collides with an obstacle will be described.
FIG. 4 is a waveform diagram showing an operation at the time of a collision. When the load machine 1 collides with an obstacle at the time t _c, the estimated disturbance torque tau _d ^#, which is calculated by the disturbance observer 9 in FIG. 1 is changed stepwise, the differentiator 10 is estimated disturbance torque differential value using Equation 1 tau Calculate _d ^# '.
[Formula 1]
τ _d ^# ′ = sτ _d ^#

Before and after the collision, the estimated disturbance torque τ _d ^# and its differential value τ _d ^# ′ change as shown in FIG. 4, for example, and when the estimated disturbance torque differential value τ _d ^# ′ exceeds the threshold value τ _th , the collision detector 11 and outputs the detected collision detection signal S _d collisions. Here, since the frequency component of the estimated disturbance torque τ _d ^# due to the collision is higher than the frequency component of the torque caused by an error in a parameter or a model used for the estimation operation of the disturbance observer 9, the estimated disturbance torque differential value τ _d ^# ′. The frequency component of the disturbance torque due to the collision becomes dominant.

In the collision controller 12 to which the collision detection signal _Sd has been input, the speed of the electric motor 2 is controlled to a safe side as in the related art to prevent the accident from spreading due to the collision. That is, the power supply to the motor 2 is forcibly stopped via the speed controller 7 and the current controller 8, or the braking is controlled so that the braking torque is generated until the motor 2 stops. Controls the electric motor 2 to move the load machine 1 in the reverse direction.

In the present embodiment, the threshold tau _th varies in accordance with the speed detection value n _m of the motor 2 used for collision detection. Specifically, as shown in FIG. 3, it is set as the threshold value tau _th increases in proportion to the constant speed range (0 to n ₁₎ the speed detection value n _m.

Thus, the estimated disturbance torque differential value tau _{d #} load machine 1 collides at high speed obstacle ^'when the amplitude is large as shown in FIG. 5 (a), the estimated disturbance torque differential value tau _d ^#' which threshold tau _th1 is large compared to, on the contrary, when the amplitude of the load machine 1 is estimated disturbance torque differential value tau _{d #} collide at low speed ^'is small as shown in FIG. 5 (b) Is smaller than the threshold value τ _th2 compared with the estimated disturbance torque differential value τ _d ^# ′.
This makes it possible to reliably detect a collision even when the load machine 1 collides at a low speed, as compared to a case where the threshold value is fixed irrespective of the motor speed, and to improve the collision detection accuracy. .

Instead of the detected speed value _nm , a speed command value output from the position controller 6 is input to the collision detector 11, and the threshold value τ _th is made proportional to the speed command value in a certain range of the speed command value. The collision may be detected by comparing with the estimated disturbance torque differential value τ _d ^# ′.
Alternatively, depending on the position command value outputted from the position detection value and position command generator 5 to be output from the position detector 3 to change the threshold tau _th, the estimated disturbance torque differential value this threshold tau _th It may be used for comparison with τ _d ^# ′.

Next, FIG. 6 is a block diagram showing a main part of a second embodiment of the present invention.
FIG. 6 (a) shows that the moving path information (speed command value, position command value, etc. for moving on a predetermined path) from the host controller 20 to the load machine 1 is input to the collision detector 11, and the moving path information By changing the threshold value (initial value τ _th0) according to the above, a pattern of the threshold value τ _th as shown in FIG. 3 is created, and this threshold value τ _th is compared with the estimated disturbance torque differential value τ _d ^# ′. may output a collision detection signal S _d obtained by the collision controller 12.
Further, as shown in FIG. 6B, a pattern of the threshold value τ _th as shown in FIG. 3 is prepared in advance by the upper controller 20 and this threshold value τ _th is inputted to the collision detector 11 and estimated. the disturbance torque differential value tau _{d #} ^'collision detection signal obtained by comparing the S _d to the collision controller 12 may output.

Next, a third embodiment of the present invention corresponding to claims 7 and 8 will be described with reference to FIG.
The time change of the disturbance torque and the collision force at the time of the collision can be detected by a frequency component extracting means such as a high-pass filter or a band-pass filter in addition to the arithmetic processing by the approximate differentiation like the differentiator 10 shown in FIG. It is.
In particular, if the load machine 1 collides with the elastic body or the like, from the collision force F _d is a spring constant K and the load machine 1 of the displacement Δx of the elastic body can be determined by Equation 2.
[Formula 2]
F _d = KΔx

Assuming that the displacement Δx in the equation (2) changes according to the constant speed x ′ of the electric motor, the equation (2) is transformed into the equation (3) if the time of collision is t = 0.
[Equation 3]
F _d = Kx't

Thus, because it has an impact force F _d is ω = Kx' [rad / s] becomes the frequency components generated by the collision of the load machine 1, to extract the frequency component omega by the high-pass filter or a band-pass filter or the like, the by comparing the results with the threshold omega _th for detecting a collision, it can be an obstacle to detect a collision with high accuracy even in an elastic body.

FIG. 7 is a block diagram showing a main part of the third embodiment, and the same parts as those in FIG. 1 are denoted by the same reference numerals.
A collision force estimating unit 9A and a frequency component extracting unit 10A such as a high-pass filter in FIG. 7 correspond to the disturbance observer 9 and the differentiator 10 in FIG. Here, the collision force estimation means 9A, using the spring constant K and the motor 2 of the speed detection value n _m of the elastic body _{(corresponding} to the speed x'in equation 3) estimates the collision force F _d by Equation 3, The frequency component ω is extracted by the frequency component extraction means 10A and input to the collision detector 11A. In the collision detector 11A, if the input frequency component ω exceeds a threshold value ω _th for collision detection (the initial value is ω _th0 ), it is determined that a collision has occurred, and a collision detection signal _Sd is output. good.
Also in the third embodiment, it is desirable to vary the threshold omega _th for detecting a collision so as to increase in proportion to the speed n _m of the electric motor 2.

  The principle of the present invention is to provide a motor control device for a servo motor, a stepping motor (a rotary stepping motor, or a direct acting type stepping motor in which a rotary stepping motor is combined with a ball screw or the like), a linear motor, and the like, and collision detection. As a method, it can be used in various manufacturing / processing / transporting fields.

1: Load machine 2: Electric motor 3: Position detector 4: Speed detector 5: Position command generator 6: Position controller 7: Speed controller 8: Current controller 9: Disturbance observer 9A: Collision force estimating means 10: Differentiator 10A: Frequency component extraction means 11, 11A: Collision detector 12: Collision controller 20: Host controller

Claims (8)

Disturbance torque estimating means for estimating a disturbance torque acting on the motor when a load machine driven by the motor collides with an obstacle, based on a torque command value and a speed of the motor;
Differentiating means for calculating a temporal change of the estimated disturbance torque output from the disturbance torque estimation means,
Collision detection means for detecting that the load machine has collided with an obstacle based on the output of the differentiating means,
Control means for controlling the speed of the electric motor to a safe side at the time of collision detection by the collision detection means,
In the motor control device provided with
Means for generating a threshold value for collision detection that changes according to the speed of the electric motor,
The motor control device according to claim 1, wherein the collision detecting means detects a collision of the load machine when an output of the differentiating means exceeds the threshold value. The motor control device according to claim 1,
An electric motor control device, wherein the threshold value is changed so as to be proportional to the speed in a constant speed range of the electric motor. The motor control device according to claim 1 or 2,
An electric motor control device, wherein the threshold value is changed according to a detected speed value of the electric motor. The motor control device according to claim 1 or 2,
An electric motor control device, wherein the threshold value is changed according to a speed command value of the electric motor. The motor control device according to any one of claims 1 to 4,
An electric motor control device, wherein the differentiating means is constituted by a filter. A disturbance torque acting on the motor when a load machine driven by the motor collides with an obstacle is estimated based on a torque command value and a speed of the motor, and the load is estimated based on a differential value of the estimated disturbance torque. In a collision detection method for detecting that a machine has collided with an obstacle,
A threshold value for collision detection to be compared with the differential value is changed according to the speed of the electric motor, and a collision of the load machine is detected when the differential value exceeds the threshold value. Collision detection method. Collision force estimating means for estimating a collision force acting on the electric motor when a load machine driven by the electric motor collides with an obstacle, based on a displacement of the load machine and a spring constant of the obstacle,
Means for extracting a predetermined frequency component of the estimated collision force output from the collision force estimation means,
Collision detection means for detecting that the load machine has collided with an obstacle based on the frequency component,
Control means for controlling the speed of the electric motor to a safe side at the time of collision detection by the collision detection means,
In the motor control device provided with
Means for generating a threshold value for collision detection that changes according to the speed of the electric motor,
The motor control device according to claim 1, wherein the collision detecting means detects a collision of the load machine when the frequency component exceeds the threshold value. When a load machine driven by the motor collides with an obstacle, a collision force acting on the motor is estimated based on a displacement of the load machine and a spring constant of the obstacle, and a predetermined frequency of the estimated collision force is estimated. In a collision detection method for detecting that the load machine has collided with an obstacle based on the component,
A threshold for collision detection to be compared with the frequency component is changed according to the speed of the electric motor, and a collision of the load machine is detected when the frequency component exceeds the threshold. Collision detection method.

Images