Servomotor control device

• US 10,409,250 B2
• Filed: 06/04/2018
• Issued: 09/10/2019
• Est. Priority Date: 06/06/2017
• Status: Active Grant

• Current Assignee: Fanuc Corporation
• Original Assignee: Fanuc Corporation
• Inventors: Shinoda, Shougo, Ikai, Satoshi
• Primary Examiner(s): Ro, Bentsu
• Application Number:
  US15/996,749
• Publication Number:
  US 20180348727A1
• Time in Patent Office: 463 Days
• Field of Search: 318625, 318432, 318434
• CPC Class Codes: G05B 19/0428 Safety, monitoring G05B19/0423 takes precedence
• CPC Class Codes: G05B 19/4062 Monitoring servoloop, e.g. overload of servomotor, loss of feedback or reference
• CPC Class Codes: G05B 19/416 characterised by control of velocity, acceleration or deceleration G05B19/19 takes precedence
• CPC Class Codes: G05B 2219/23416 Enter application program into I-O module, like motion program, servo program
• CPC Class Codes: G05B 2219/23431 Change program on detection of deviations
• CPC Class Codes: G05B 2219/37582 Position, angle of workpiece surface
• CPC Class Codes: G05B 2219/41166 Adaptive filter frequency as function of oscillation, rigidity, inertia load
• CPC Class Codes: G05B 2219/42327 Detect ballscrew wear
• CPC Class Codes: G05B 2219/50049 Control machine as function of position, angle of workpiece

Abstract

A servomotor control device includes: a servomotor; a driven body that is driven by the servomotor; a connection mechanism that connects the servomotor and the driven body; a first position detection section that detects a position of the servomotor; a second position detection section that detects a position of the driven body; and a motor control unit, in which the motor control unit includes: a force estimation section that estimates a drive force acting on the driven body at a connection part between the connection mechanism and the driven body; a rigidity estimation section that estimates a magnitude of rigidity of the connection mechanism based on a detected position of the servomotor, a detected position of the driven body, and an estimated drive force; and a rigidity variation detection section that detects a change in rigidity of the connection mechanism, based on the estimated magnitude of rigidity.

Claims

1. A servomotor control device comprising:

a servomotor;

a driven body that is driven by way of the servomotor;

a connection mechanism. that connects the servomotor and the driven body to transmit power of the servomotor to the driven body;

a first position detection section that detects a position of the servomotor;

a second position detection section that detects a position of the driven body; and

a motor control unit that controls the servomotor,

wherein the motor control unit includes;

a force estimation section that estimates a drive force acting on the driven body at a connection part between the connection mechanism and the driven body;

a rigidity estimation section. that estimates a magnitude of rigidity of the connection mechanism based on a position of the servomotor detected. by the first position detection section, a position of the driven body detected. by the second position detection section, and a drive force estimated by the drive force estimation. section; and

a rigidity variation detection section that detects a change in rigidity of the connection mechanism, based on the magnitude of rigidity estimated by the rigidity estimation section.

2. The servomotor control device according to claim 1, wherein the rigidity estimation section estimates a magnitude of rigidity (R) of the connection mechanism according to Formula (1), based on a difference (δ

) between a position of the servomotor detected by the first position detection section and a position of the driven body detected by the second position detection section, and a drive force (T) estimated by the force estimation section;

Magnitude of rigidity (R)=drive force (T)/difference (δ

) between position of servomotor and position of driven body 

 

(1).

3. The servomotor control device according to claim 1, wherein the rigidity variation detection section detects deterioration of the connection mechanism as a change in rigidity of the connection mechanism, when the magnitude of rigidity estimated by the rigidity estimation section declines to no more than a predetermined threshold.

4. The servomotor control device according to claim 3, wherein the motor control unit further includes a notification section that notifies information indicating deterioration detected by the rigidity variation detection section, or information indicating the magnitude of rigidity detected by the rigidity estimation section.

5. The servomotor control device according to claim 4, wherein the notification section is a display device which displays information.

6. The servomotor control device according to claim 1, wherein the motor control unit further includes a storage section that stores, as rigidity data, a plurality of magnitudes of rigidity estimated by the rigidity estimation section at every predetermined time interval or indefinite time interval, and

wherein the rigidity variation detection section detects a change and variation in rigidity of the connection mechanism based on the rigidity data stored in the storage section.

7. The servomotor control device according to claim 6, wherein the motor control unit further includes a notification section that notifies information indicating at least one of the change and a variation in rigidity detected by the rigidity variation detection section, or information indicating the magnitude of rigidity detected by the rigidity estimation section.

8. The servomotor control device according to claim 1, wherein the motor control unit further includes:

a dual position control section that performs semi-closed feedback control based on a high-frequency component of a first deviation between a position command value for drive control of the servomotor and the position of the servomotor detected by the first position detection section, and full-closed feedback control based on a low-frequency component of a second deviation between the position command value and the position of the driven body detected by the second position detection section; and

a control proportion varying section that causes a proportion of the semi-closed feedback control to increase, and a proportion of the full-closed feedback control to decrease in the dual position control section, in response to a decline in the rigidity of the connection mechanism estimated by the rigidity estimation section.

9. The servomotor control device according to claim 8, wherein the dual position control section further includes:

a first subtraction part that obtains a first deviation between the position command value and the position of the servomotor detected by the first position detection section;

a second subtraction part that obtains a second. deviation between the position command value and the position of the driven body detected by the second position detection section;

a high-pass filter that inputs the first deviation from the first subtraction part;

a low-pass filter that inputs the second deviation from the second subtraction part; and

an addition part that adds a high-frequency component of the first deviation outputted from the high-pass filter and a low-pass component of the second deviation outputted from the low-pass filter, and

wherein the control proportion varying section causes a cut-off frequency of the high-pass filter and a cut-off frequency of the low-pass filter to decline in response to a decline in the rigidity of the connection mechanism.

10. The servomotor control device according to claim 9, wherein the control proportion varying section sets the cut-off frequency of the high-pass filter and the cut-off frequency of the low-pass filter to be the same.