METHOD OF DETECTING COLLISION OF MOVING PORTIONS DRIVEN BY SERVO MOTOR

Abstract: A method of quickly detecting collision of a robot's moving portion, such as an arm driven by a servo motor with an external object in order to prevent or reduce damage of the machine caused by collision. A digital signal processor of an axis controller (2) which constitutes a software servo calculates a speed instruction and a torque instruction based on a movement instruction from a main computer (1) to periodically execute servo control for the axis of the robot, and further determines whether the torque instruction or a speed deviation between the speed instruction and the actual speed of the servo motor (13b) is greater than a predetermined value or not. When the torque instruction or the speed deviation is greater than the predetermined value, it is determined that the robot's moving portion (13a) has come into collision with an external object, and the servo motor is stopped.

Description: The present invention relates to a collision detecting method capable of prompt detection of a collision of machine operating parts driven by servomotors with a foreign object. During operation of a machine, if a foreign object entering the working region of a machine operating part interferes with the same operating part, there occurs a possibility of impeding normal operation or damaging the machine. Conventionally, various methods have been employed to avoid such an inconvenience. For example, a collision of a machine operating part with a foreign object is detected by using a touch sensor mounted to the outer surface of the machine operating part. The use of the sensor, however, entails an increase of the cost of the machine, and it is not possible to detect a collision of the machine operating part with a foreign object occurring outside the operable region of the sensor. In those types of machines in which the machine operating parts thereof are driven by servomotors, it is known to detect a collision of a machine operating part with a foreign object by detecting an increase of the driving current for the corresponding servomotor in excess of a predetermined discrimination value due to the collision. However, there is a passage of time from the detection of a collision until the servomotor driving current actually reaches the discrimination value, and therefore, a time lag in collision detection is inevitable and thus the collision can bring about a greater damage. In other types of machines, e.g., industrial robots, a collision discriminating process is carried out by a built-in computer in accordance with a sensor output or the detected value of the servomotor driving current. Namely, in machines of this type, the collision discriminating process is executed by the computer which is used for the robot control; in other words, both the collision discriminating process and the robot control are carried out by the same computer. The robot control, however, includes a variety of complicated data processings, and accordingly, the periods of the control process and the collision discriminating process of the computer become inevitably long. Consequently, time is required between the collision detection and the completion of the collision discriminating process, thus further increasing the time lag of collision detection. An object of the present invention is to provide a method which is capable of promptly detecting a collision of machine operating parts driven by servomotors with a foreign object and preventing or lessening a damage of a machine, etc., due to the collision. It is already known to detect an abnormal operation of a machine operating part driven by a motor according to the preamble of the attached claim 1. Thus see US-A-4,698,773 and GB-A-2,140,951. According to the present invention there is provided a method of detecting an abnormal operation of a machine operating part driven by a servomotor, comprising the steps of: (a) detecting a predetermined parameter relating to a driving force of the servomotor at first predetermined periods; and (b) determining that an abnormal operation of the machine operating part has occurred when the detected predetermined parameter becomes greater than a predetermined value; characterised in that said abnormal operation of the machine operating part is a collision thereof, and said detected predetermined parameter is the amount of change of a parameter over a second predetermined period, this parameter comprising either a torque command value supplied to the servomotor, or a deviation between a velocity command supplied to the servomotor and an actual velocity of the servomotor. Preferably, the first predetermined period at which the predetermined parameter is detected, is set to a value shorter than a period of machine control action. Further, the detected predetermined parameter is preferably the difference between values of the torque command or velocity deviation detected in a previous period and a current period, with the value of the second predetermined period being equal to that of the first predetermined period. According to the present invention, a collision of the machine operating part driven by the servomotor is detected when the amount of change of a collision discriminating parameter, which is detected at predetermined periods, becomes greater than the predetermined value. Therefore, a collision of the machine operating part can be promptly detected without the need to use a special sensor, and accordingly, a damage of the machine, etc. due to the collision can be prevented or lessened. Brief Description of the Drawings FIG. 1 is a block diagram showing a principal part of an industrial robot to which a collision detecting method according to an embodiment of the invention is applied; and FIG. 2 is a block diagram showing, in terms of transfer functions, the control characteristic of a servo system for one axis of the robot. In FIG. 1, an industrial robot has an arrangement fundamentally similar to a conventional one, and comprises a robot mechanism 13 for performing predetermined tasks on a subject 14 having various sensors (not shown), a control unit having various elements described hereinafter, for controlling the robot mechanism 13, and various sensors (not shown). The robot mechanism 13 has various operating parts, e.g., a set of arms, and an end effector mounted to the distal end of the arm. Each of the operating parts (only one is shown with numeral 13a) is moved linearly or rotated relative to one or more axes thereof by an associated actuator or actuators. The actuators each comprise a servomotor (only one is shown with numeral 13b) which is preferably provided with a velocity detector 13c. The control unit comprises a main computer 1 for robot control, which includes a microprocessor, a ROM storing a control program executed by the microprocessor, a RAM for temporary storage of data, etc., and is arranged to deliver a movement command for the individual axes of the robot. An axis controller 2 cooperates with the individual servomotors of the robot and servo amplifiers associated therewith (one of which is illustrated with numeral 12), to constitute digital servo systems for the respective axes, i.e., software servo systems. Specifically, the axis controller 2 has a digital signal processor (not shown) having a position control function 21, a velocity control function 22, and an integrating function 24 shown in FIG. 2, and is arranged to deliver a torque command in accordance with the movement command supplied from the main computer 1, to a servo amplifier which cooperates with the associated servomotor to provide a control characteristic 23 shown in FIG. 2. In FIG. 2, symbols Kp and Kv respectively denote the position loop gain and the velocity loop gain of the axis controller 2, Kt denotes the torque constant of the servomotor, and J denotes the total inertia acting on the output shaft of the servomotor. The control unit further comprises an I/O unit 3 connected to the various sensors of the robot, the various actuators, and the various sensors of the work subject 14; a serial port 4 connected to a teaching pendant 7 and an operation panel 9; a RAM 5 as a data memory with a backup battery 8; and a power circuit 6 to which electric power is supplied through an input transformer 11 and an input unit 10. Further, the control unit is so arranged that electric power is supplied to the main computer 1 and the above elements 2 - 5 connected to the computer 1, through the power circuit 6, and to the robot mechanism 13 through the input unit 10, and that electric power is also supplied to the servo amplifiers 12 through the input unit 10 and the servo transformer 15. The operation of the robot constructed as above will now be described. Before starting the robot, an operator operates the teaching pendant 7 and the operation panel 9, to teach the robot in a conventional manner, whereby the teaching data is stored in the RAM 5. When the robot is operated, the main computer 1 successively executes various control actions at predetermined periods. The control actions include, e.g., a process of calculating movement commands for the individual axes of the robot in accordance with the teaching data stored in the RAM 5. The digital signal processor of the axis controller 2 carries out position control, velocity control and torque control (servo control action) for the individual axes of the robot and a collision discriminating action mentioned later, at periods shorter than those of the control actions by the main computer 1. Namely, in a given control action period, the digital signal processor integrates the output of the velocity detector 13c mounted on a servomotor 13b, as shown by reference numeral 24 in FIG. 2, which output relates to the aforesaid control action period and represents the velocity V of this servomotor, so as to calculate an actual rotational position Pf of the servomotor 13b, and then calculates a velocity command Vc for this servomotor in accordance with the difference (position deviation) εp between the command position (movement command) Pc from the main computer 1 and the actual motor position Pf. The digital signal processor then calculates a torque command Tqc in accordance with the difference (velocity deviation) εv between the calculated velocity command Vc and the actual motor velocity V, subjects the calculated torque command Tqc to D/A conversion, and outputs the resultant to the servo amplifier 12. The servo amplifier 12 causes a driving current corresponding to the torque command Tqc to flow through the servomotor 13b, to rotate the servomotor at the velocity V, thereby driving the associated robot operating part 13a. During operation of the robot, the digital signal processor of the axis controller 2 carries out the collision discriminating action preferably in each of the above-mentioned periods of the servo control action (generally, once per several periods of the servo control action). Namely, the difference |εv - εv′| between the velocity deviation εv′ calculated in the previous period (generally, calculated several periods before) and the velocity deviation εv calculated in the current period, or the difference |Tqc - Tqc′| between the torque command Tqc′ calculated in the previous period and the torque command Tqc calculated in the current period (generally, the amount of change of the velocity deviation or of an equivalent torque command within a plurality of periods of the servo control action) is obtained. Then, it is determined whether the obtained difference (the amount of change) is greater than the corresponding predetermined value, which is set to a value greater than the maximum value which can be assumed during normal operation of the robot. If the deference (the amount of change) |εv - εv′| or |Tqc - Tqc′| is smaller than the corresponding predetermined value, it is judged that no collision has occurred between the robot operating part 13 and a foreign object, and the velocity deviation εv or Tqc calculated in the current period is stored in the corresponding register in the digital signal processor, thus completing the collision discriminating action for the current period. If the robot operating part 13a collides, e.g., with a foreign object which has entered the working region of the operating part, the actual rotational velocity V of the servomotor 13b decreases. In such a case, the velocity deviation εv (= Vc - V) calculated by the digital signal processor in the period of servo control action immediately after the collision is suddenly increased and thus the torque command Tqc sent by the processor is also suddenly increased. Accordingly, in this period of the servo control action (generally, one to several periods later), the difference (the amount of change) |εv - εv′| or |Tqc - Tqc′| becomes greater than the corresponding predetermined value, whereby the digital signal processor determines that a collision has occurred. In this case, the processor sets the value Tqc of the torque command register to "0", for example, so that no driving current is supplied to the servomotor 13b and thus the motor rotation is stopped. By the collision detection described above, a collision can be detected in a period of the servo control action immediately after the occurrence of the collision (generally, several periods later), and accordingly, the time lag in collision detection is equal to one period of the servo control action (generally, several periods) of the digital signal processor at the maximum, whereby the collision detection is faster than in the case of using the main computer 1 for this purpose.

Claims: A method of detecting an abnormal operation of a machine operating part driven by a servomotor, comprising the steps of: (a) detecting a predetermined parameter relating to a driving force of the servomotor at first predetermined periods; and (b) determining that an abnormal operation of the machine operating part has occurred when the detected predetermined parameter becomes greater than a predetermined value; characterised in that, said abnormal operation of the machine operating part is a collision thereof, and said detected predetermined parameter (|εv - εv′| or |Tqc - Tqc′|) is the amount of change of a parameter (εv or Tqc) over a second predetermined period, this parameter (εv or Tqc) comprising either a torque command value (Tqc) supplied to the servomotor, or a deviation (εv) between a velocity command (Vc) supplied to the servomotor and an actual velocity (V) of the servomotor. A method according to claim 1, wherein said first predetermined period is set to a value shorter than a period at which various control actions including an output of a movement command to said servomotor are carried out by a main computer of a machine to which said method is applied. A method according to claim 1 or 2, wherein said second predetermined period is set to a value equal to said first predetermined period, and said amount of parameter change is the difference between a value of the collision discriminating parameter detected in a previous period and a value of the same parameter detected in a current period.

Inventors: HAMURA MASAYUKI ROOM - FANUC M, KATO TETSUAKI ROOM - FANUC MAN, NAITO YASUO ROOM - FANUC MANSH, TORII NOBUTOSHI, HAMURA, MASAYUKI ROOM 8-103, FANUC MANSHONHARIMOMI, KATO, TETSUAKI ROOM 8-101, FANUC MANSHONHARIMOMI, NAITO, YASUO ROOM 7-205, FANUC MANSHONHARIMOMI, TORII, NOBUTOSHI
Applicants: FANUC LTD, FANUC LTD.
Application year: 1989
Main classification: G05B19
IPC codes: G05B19, B25J19, B25J9, G05D1