JP2580190B2 - Abnormality detection device for industrial robots - Google Patents

Description

Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a servo abnormality detection apparatus for an industrial robot, and more particularly to a servo abnormality detection apparatus suitable for promptly detecting an abnormal operation of a robot main body.

[Conventional technology]

An abnormality in the servo system that drives each movable part of the industrial robot causes an abnormal operation of the robot main body and causes a runaway, which is very dangerous. For this reason, it is recognized that the servo abnormality can be detected with good response from the operation state of the servo system. Conventionally, as a method for detecting such an abnormality of the robot servo system, as described in Japanese Patent Application Laid-Open No. 24305/1987, the absolute value of the deviation between the position command value and the position detection value of each movable part servo system is described. There is known a method of counting a time when a value exceeds a predetermined deviation reference value and detecting a servo abnormality when the time continuously exceeds a predetermined time reference value.

[Problems to be solved by the invention]

In such a method, a servo error is detected only by the position deviation of the servo system.Therefore, it takes time from the occurrence of the error in the servo system to the detection of the fact that the position of the robot movable unit is not moving as instructed. As a result, runaway of the robot body due to an abnormality in the servo system cannot be prevented with good response.
As described in JP-A-61-152351, the load torque can be detected from the magnitude of the motor current, and the abnormality can be detected based on the value of the detected load torque. However, in the method of detecting the load torque from the magnitude of the motor current, if the speed of the motor is accelerated or decelerated as in the case of a robot arm, the load torque becomes abnormal even during acceleration / deceleration, and the robot arm receives another load. When the vehicle collides with an object, the load torque becomes indistinguishable.

An object of the present invention is to provide an abnormality detection device for an industrial robot that can reliably detect a servo abnormality by an abnormality in a load torque regardless of the operation state of the robot.

[Means for solving the problem]

In order to achieve the above object, the present invention provides an industrial robot, comprising: a driving source that rotationally drives a movable unit; and a servo control unit that servo-controls the driving source by an operation command for the movable unit. A rotation position detection unit that detects a rotation position of the drive source corresponding to the operation command as a state amount of the source; a rotation speed detection unit that detects a rotation speed of the drive source as a state amount of the drive source; Torque command value calculation means for calculating a value corresponding to the drive torque of the drive source from the detected value of the rotational position detection means and outputting it as a torque command value; and an estimated value of drive torque for the drive source from the torque command value And a load torque estimation value calculation means for calculating a load torque estimated as a component for compensating a load torque applied to the movable portion of the torque output by the drive source. Speed estimation value calculating means for estimating the rotational speed of the drive source by integrating the deviation between the value and the calculated value of the drive torque calculating means; the detected value of the rotational speed detecting means and the calculated value of the speed estimated value calculating means And a load torque estimated value calculated by the load torque estimated value calculated by the load torque estimated value calculating means. Time measurement means for measuring a load torque estimated value duration that continues beyond a predetermined reference value; and servo abnormality detection means for outputting a servo abnormality signal when the load torque estimated value duration exceeds a predetermined reference time. And a servo abnormality detecting device for an industrial robot.

[Action]

According to the above-described means, a drive torque command (current command value) is generated from the rotational position of the drive source and the operation command, and an estimated value of the drive torque is calculated from the drive torque command.
The difference between the estimated load torque and the estimated drive torque as a component that compensates for the load torque based on the rotational speed of the drive source is determined as a component that accelerates the drive source, and this component is integrated. The speed of the driving source is estimated. Then, when an error between the estimated speed of the drive source and the rotational speed of the drive source is calculated, this value is obtained as an estimated value of the load torque acting on the movable part. The duration during which the estimated load torque value exceeds a predetermined reference is measured, and when the measured value exceeds the predetermined reference time, a servo abnormality signal is output as a servo system abnormality. Therefore, the true load torque can be detected irrespective of the operation state of the robot, and the servo abnormality can be reliably detected based on the abnormality of the load torque.

〔Example〕

A first embodiment of the present invention will be described with reference to FIG. FIG. 1 is a block diagram of each drive source servo system of an industrial robot. 1 in FIG. 1 is a block diagram showing software processing of each drive source servo system, which is executed at a predetermined sampling cycle as a software program of a micro computer described later. Note that various means as constituent elements of the present invention are included in the microcomputer. Reference numeral 2 denotes a D / A (Digital to Analog) converter for converting a calculation result of the digital servo processing into an analog amount, and outputs a current command value which is a torque command value. Reference numeral 3 denotes a servo amplifier for supplying a drive current to a motor 5 which is a drive source in accordance with a current command value, and executes a current control using an output of a current detector 4 for detecting the motor current. I have. Numeral 6 denotes a movable portion driven by the motor 5 as a drive source, and the movable portion is actually connected to form an industrial robot main body. A rotary encoder 7 for detecting the position and speed of the motor 5 is attached to the rotating shaft of the motor 5. The counter 8 detects the motor rotation position by counting the number of detection pulses. Is calculated.

In the block diagram showing the software processing of the servo system, in FIG. 1, reference numeral 103 denotes a rotational position detecting means for detecting the rotational position of the motor 5 corresponding to the position command and the rotational speed of the motor 5 as the state quantity of the motor (drive source) 5. And a rotation speed detecting means for detecting the rotation speed of the motor 5 using the count value of the counter 8.
A part for calculating the position θ _m and the speed ω _m of FIG. on the other hand,
The position command value θ _mr for the motor is _{fetched at} every predetermined sampling period, and using the position command value θ _mr and the position and speed detection values θ _m and ω _m , the position servo of the motor 5 as the drive source is performed. The processing is executed. First, a deviation between the position command value θ _mr and the position detection value θ _m is calculated, and a compensation element 101 in the position control system is applied thereto. As a result, a speed command value ω _mr is obtained. Normally, a proportional gain is used as the compensation element C _p (S), but phase lag compensation or the like is also used in order to remove the offset of the position system. That is, Is selected as follows. Here, kp is a proportional gain of the position control system, and T ₁ and T ₂ are time constants of a phase delay element. Also, S
Is a Laplace operator, and describes the compensation element Gp (S) as a transfer function in a continuous system. In the case of executing by digital processing of a micro computer, it is sufficient to convert to a processing algorithm of a discrete time system in consideration of the sampling period. For example, "Digital signal processing and control" by Kimura (Showaido, 1982 ).

Next, the output ω _mr of the compensation element of the position control system is subtracted from the speed detection ω _{m of the} motor, and the compensation element 102 in the speed control system is applied. As the compensation element Gs (S), proportional integral compensation is usually used. Is chosen as Here, Ks is a proportional gain of the speed control system, and T _I is an integration time constant. This compensation operation can be converted into a processing algorithm of a discrete time system as in the case of the position control system, and this is executed by software processing of a micro computer.

As a result, the current command value im corresponding to the drive torque command
r is generated. This current command value imr is input to the servo abnormality determination processing unit 104. This servo abnormality determination processing unit 1
04 is equipped with load torque estimation and servo abnormality detector,
The drive unit performs the servo abnormality determination using the load torque output for the load applied to the movable unit. The current command value imr calculated by the speed control process is multiplied by a coefficient _{T at} 105, and the estimated value of the driving torque for the motor is obtained. Becomes Here, _T is an estimated value of the motor torque constant. this Motor speed estimated value by integrating the value obtained by adding the correction term of the state estimator to Get. Here, the transfer function of 106 is described as 1 / (eS), Je is the estimated value of the drive system inertia moment in terms of the motor shaft, and S is the Laplace operator. Motor speed estimate obtained in this way And the error between the motor speed detection value ω _m and 107,
As a result of performing the compensation of 108, the above-described correction value of the state estimator is calculated. Here, the transmission function of 107 is K ₁ / S, 108
Is k ₂ , and k ₁ and k ₂ are the outputs of the state estimator Is the state estimation gain for matching the output of the actual plant (the detected motor speed value ω _m ), and S is a Laplace operator. At this time, the estimated value of the load torque Can be obtained as a value obtained by executing the compensation of 107 for the deviation. That is, 105 is a drive torque estimated value calculating means for estimating the drive torque for the motor 5, and 106 is
107, 108 as speed estimation value calculating means for estimating the speed of
Are configured as load torque estimated value calculation means for estimating a component for compensating the load torque of the motor 5. A summary of these load torque estimation algorithms can be described as follows.

Note that the above equation (5) indicates a load torque estimated value when k ₂ shown in the equation (4) is sufficiently small (the output of 108 is sufficiently small), that is, a component for compensating the load torque. .

The operation including such integration is converted into a processing algorithm of a discrete time system as in the case of the servo system described above,
It is executed by software processing of a micro computer. The estimated load torque obtained in this way In step 109, the presence or absence of an abnormality in the servo system can be detected.

In this embodiment, the case where only the moment of inertia e is taken into account as a model of the drive system including the motor is shown. However, in the case of a drive system in which the influence of the friction torque is large,
A load torque estimator can be similarly constructed by adding a friction coefficient term to the 106 elements. In the case where the influence of the delay of the current control system is taken into account in the power amplifier shown in FIG. 3, it can be similarly realized by giving the element 105 a first-order delay characteristic.

Next, FIG. 2 shows a hardware configuration of the first embodiment. Here, the means 1 to 8 are the same as those described with reference to FIG. 1. Among them, the basic servo processing and the load torque estimation algorithm described above are executed by the software processing of the micro computer 1. Reference numeral 9 denotes a timer for giving the microcomputer 1 an interrupt signal INT at regular intervals, and the sampling cycle of the servo processing is determined based on this signal.

FIG. 3 shows a timing chart of such software processing. Where INT is the interrupt signal for the microcomputer, and the period between T _I. In the interrupt processing of the microcomputer, the servo processing as described above is executed. Here, the sampling cycle of the position control process and the speed control process is a short cycle of the speed control system, and the position control system can execute a stable and responsive servo process even if the position control system processes the cycle several times the cycle. FIG. 3 shows the processing timing when a single microcomputer executes servo processing of a plurality of robot axes. If the number of axes is n, the speed control is performed for all axes in response to one interrupt. (N axis),
If the position control is sequentially executed one by one of the n axes, the sampling cycle for that particular servo axis can be set such that the speed control cycle is equal to the interrupt cycle, T _I , and the position control cycle is nT _I. Here, the hatched portions in FIG. 4 indicate the timing of processing one specific axis of the n-axis servo processing.

Next, a flowchart of the servo algorithm processed at such timing is shown in FIG. Here, the position control process and the speed control process are executed at different sampling periods. First, in the position control process, S11
Captures the position command value theta _m r at position detection value at S12 theta
_m is detected. In S13, a deviation between the two is calculated, and in S14, a compensation calculation of the position control system is performed. As a result, the speed command value ω _mr is output in S15. On the other hand, in the speed control process, the W _mr calculated by the position control system is fetched in S21, and the detected speed value ω is
_m is detected. In S23, a deviation between the two is calculated, and in S24, a compensation calculation of the speed control system is executed. As a result, a current command value imr is output in S25. On the other hand, this imr and the speed detection value ω _m
The load torque estimation process is executed in S26 using the Is calculated. Using this value, a servo abnormality detection process is executed in S27.

Next, the estimated value of the load torque FIG. 5 shows a flow chart from the time when is obtained until the servo abnormality determination is executed. This process Is executed for each calculation of. First, the estimated value of the load torque from the current command value imr and the motor speed detected value omega _m of the motor in S101 Is calculated. Next, in S102, the absolute value If, by comparing the magnitude of the load torque reference value tau _dn to be preset, the process proceeds to determine the magnitude of the load torque estimate is not Zabo abnormal if within the reference value S103. Here, the magnitude of the estimated value of the load torque There is cleared to zero a variable Na for counting the size Katsuta number than the reference value tau _dn at every sampling period. Next, this processing is returned in S104. on the other hand, In the case of, the variable Na is increased by 1 in S105, Is counted (sampling cycle XNa). Then, the count value Na is determined whether does not exceed the reference value N _an, which is set in advance in S106, returns at S107 if it is within the reference value. On the other hand, When the number of times a has decreased to or larger than the reference value N _an,, determines that the servo abnormality, performs the processing of the servo abnormality detected in S108.
Thereafter, the emergency stop process is immediately executed, and the process returns in S110.

As described above, the embodiment has been described with respect to the estimation of the load torque and the servo abnormality detection method using the estimated value for one axis of the servo system of each movable part of the industrial robot. This process is executed for each servo axis. Needless to say, At this time, the load torque reference value tau _dn and time reference value N _an, for detecting servo abnormality is set as the optimum value for each servo system. Here, since Na is a count value of the abnormal state of the load torque in each sampling cycle, it is used as a time reference value for servo abnormality determination.

As described above in detail, according to the first embodiment, of the current command values for the motors that drive the movable parts of the industrial robot, the components used for accelerating the motor and the components for compensating for the load torque are separated. Therefore, the abnormality of the disturbance torque acting on the servo system can be detected with good response regardless of the acceleration state of the motor. Here, the component used for accelerating the motor is a component of the torque generated by the motor (proportional to the current command value imr for the motor) excluding load torque components such as friction torque and gravity torque. The components input to 106 (outputs from 105 to 107 and 108
And the components obtained by subtracting the outputs of The motor speed, that is, the rotation speed of the robot arm changes according to this value. When this value is large, the motor speed changes at a large acceleration. Conversely, when this value is small, the motor speed changes at a small acceleration.

On the other hand, the component for compensating the load torque is a component used to overcome load torque such as friction torque and gravitational torque out of the torque generated by the motor.
, And the output and the output of 108. here,
Usually, the output of 108 is sufficiently small, and if this value is ignored, the output of 107 corresponds to the component that compensates for the load torque. Then, a value obtained by removing the component for compensating the load torque from the torque generated by the motor is used to accelerate or decelerate the motor. Thus, the result obtained by subtracting the output of 107 corresponding to the load torque from the output of 105 corresponding to the motor drive torque corresponds to the component (acceleration / deceleration torque) used for acceleration and deceleration of the motor.

Summarizing these relationships, motor drive torque-load torque = acceleration / deceleration torque. Thus, motor driving torque−acceleration / deceleration torque = load torque. Then, since the value obtained by removing the acceleration / deceleration torque from the motor drive torque corresponds to the load torque, the load torque acting on the robot arm is detected based on the current command value and the motor speed detection value,
The presence or absence of an abnormality in the servo system is detected based on the magnitude of the detection value. Such abnormality detection is an important function as a means for promptly detecting a dangerous state in which the robot is operating by contacting or colliding with an obstacle in the work environment.

Further, in the first embodiment, a model of a motor drive unit is provided for load torque estimation, and a motor speed corresponding to a current command (ie, drive torque command) for the motor is estimated.
A method of estimating the load torque from the estimated speed value and the detected speed value is used. For this reason, if an abnormal motor current flows even though a forward-facing element of the servo system (for example, a D / A converter, etc.) fails and software issues a correct current command, When the speed estimation value in the load torque estimation portion largely deviates from the motor speed detection value, it can be equivalently detected as an abnormality in the load torque estimation value. In the conventional servo error detection method, in the case of a failure of the D / A converter, first, the servo system goes out of control, and as a result, the position error increases, thereby detecting the servo error. On the other hand, according to the method of the present invention, since the abnormality of the forward element of the servo system can be detected more responsively, there is an effect that runaway of the robot body can be prevented.

Next, a second embodiment of the present invention is shown in FIG. Industrial robots are classified into several modes of operation. That is, there are a stop state at power-on, a manual operation state for teaching the robot to operate, and an automatic operation state at the time of playback. Each of these operation modes can be selected by the operation unit of the industrial robot.
At this time, the stop state is a stop state of the motor when a start or stop command is not sent to the motor in a state where power is supplied to the power circuit for driving the drive source. This is basically different from the stop state by the stop command. Therefore, by setting the reference values (load torque reference value and time reference value) for detecting the servo abnormality according to the operation mode of the robot according to the command from the operation unit, the servo can be set at the optimum detection level. An abnormal state can be determined. According to this embodiment, at the time of manual operation requiring the safety of the operator, safer control can be realized by reducing the servo abnormality detection level.

〔The invention's effect〕

According to the present invention, a drive torque command value is generated from the rotational position of the drive source and the operation command, an estimated value of the drive torque for the drive source is calculated from the drive torque command value, and the rotational speed of the drive source is calculated. The difference between the estimated load torque and the estimated drive torque as a component to compensate for the load torque is integrated to estimate the speed of the drive source, and the error between the estimated speed of the drive source and the rotational speed of the drive source is calculated. Is calculated to calculate an estimated value of the load torque, and the duration of the estimated load torque value exceeding a predetermined reference is measured. When the measured value exceeds the predetermined reference time, a servo system abnormality is detected. Therefore, the true load torque can be detected irrespective of the operation state of the robot, and the servo abnormality can be reliably detected by the abnormality of the load torque.

[Brief description of the drawings]

FIG. 1 is a configuration diagram for explaining a servo process according to a first embodiment of the present invention, FIG. 2 is a hardware configuration diagram of the first embodiment, FIG. 3 is a timing diagram of the servo process, and FIG. FIG. 5 is a flowchart of servo processing, FIG. 5 is a flowchart of servo abnormality detection, and FIG. 6 is an explanatory diagram showing a method of setting a reference value for servo abnormality detection in the second embodiment of the present invention. 1 ... microcomputer, 2 ... A / D converter, 3 ...
... power amplifier, 4 ... current detector, 5 ... motor, 6
... Robot movable part, 7 ... Encoder, 8 ... Counter, 101 ... Position control compensation, 102 ... Speed control compensation, 103
...... Position and speed detection processing, 104 ... Load torque estimation processing.

 ──────────────────────────────────────────────────続 き Continued on the front page (72) Inventor Hiroyuki Hanei 7-1-1 Higashi Narashino Plant, Narashino City, Chiba Prefecture Inside the Hitachi, Ltd. Narashino Plant (72) Inventor Raji Shimomura 7-1-1 Higashi Narashino City, Narashino City, Chiba Prefecture No. 1 Inside the Narashino Plant of Hitachi, Ltd. (56) References JP-A-61-152351 (JP, A) JP-A-57-160380 (JP, A)

Claims (2)

(57) [Claims] 1. An industrial robot comprising: a drive source for rotatingly driving a movable portion; and servo control means for servo-controlling the drive source in accordance with an operation command for the movable portion. Rotation position detection means for detecting the rotation position of the drive source corresponding to the command, rotation speed detection means for detecting the rotation speed of the drive source as the state quantity of the drive source, detection of the operation command and the rotation position detection means And a torque command value calculating means for calculating a value corresponding to the drive torque of the drive source from the value and outputting as a torque command value, and a drive torque estimated value for calculating an estimated value of the drive torque for the drive source from the torque command value Calculating means for calculating a load torque estimated value calculated as a component for compensating a load torque applied to the movable portion of the torque output by the drive source, and calculating the drive torque Speed estimation value calculating means for estimating the rotational speed of the drive source by integrating the deviation from the calculated value of the step, and calculating an error between the detected value of the rotational speed detecting means and the calculated value of the speed estimated value calculating means. Load torque estimated value calculating means for calculating the load torque estimated value and outputting the calculated value to the speed estimated value calculating means; and the load torque estimated value calculated by the load torque estimated value calculating means exceeds a predetermined reference value. Measuring means for measuring the duration of the estimated load torque value that lasts, and servo abnormality detection means for outputting a servo abnormality signal when the duration of the estimated load torque value exceeds a predetermined reference time. Industrial robot servo abnormality detection device. 2. The apparatus according to claim 1, wherein the predetermined reference value and the predetermined reference time are set according to an operation mode of the industrial robot. .