WO1990001732A1 - Impingement detection/driving stop method in machine driven by servo motor - Google Patents

Impingement detection/driving stop method in machine driven by servo motor Download PDF

Info

Publication number
WO1990001732A1
WO1990001732A1 PCT/JP1989/000821 JP8900821W WO9001732A1 WO 1990001732 A1 WO1990001732 A1 WO 1990001732A1 JP 8900821 W JP8900821 W JP 8900821W WO 9001732 A1 WO9001732 A1 WO 9001732A1
Authority
WO
WIPO (PCT)
Prior art keywords
collision
value
detection
servo motor
speed
Prior art date
Application number
PCT/JP1989/000821
Other languages
English (en)
French (fr)
Inventor
Nobutoshi Torii
Yasuo Naito
Masayuki Hamura
Kazuhisa Otsuka
Original Assignee
Fanuc Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fanuc Ltd filed Critical Fanuc Ltd
Publication of WO1990001732A1 publication Critical patent/WO1990001732A1/ja
Priority to KR1019900700628A priority Critical patent/KR900702425A/ko

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B9/00—Safety arrangements
    • G05B9/02—Safety arrangements electric
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/406—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by monitoring or safety
    • G05B19/4061—Avoiding collision or forbidden zones
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34047—Dsp digital signal processor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/36—Nc in input of data, input key till input tape
    • G05B2219/36399—On excess error or on release joystick stop movement, dead man, shut off motors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/37—Measurements
    • G05B2219/37624—Detect collision, blocking by measuring change of velocity or torque
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/49—Nc machine tool, till multiple
    • G05B2219/49162—On collision, obstruction reverse drive, accelerate, cancel inertia
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/50—Machine tool, machine tool null till machine tool work handling
    • G05B2219/50179—Dynamic tolerance, limit values as function of speed, type of command

Definitions

  • the present invention relates to a collision detection method capable of quickly detecting a collision between a machine operating unit driven by a servomotor and a foreign object and stopping the driving of the machine operating unit upon detection of the collision. .
  • a collision sensor between a machine operating unit and a foreign object is detected using a touch sensor attached to the outer surface of the machine operating unit.
  • the cost of the machine is increased with the installation of the sensor, and the collision between the machine operating part and the foreign object outside the operating area of the sensor cannot be detected.
  • the collision between the mechanical operating part and a foreign object is detected when the driving current of the servo motor exceeds a predetermined discrimination value due to the collision. ing.
  • sensor output or servo motor drive Known is a type of machine, such as an industrial lopot, that performs a collision determination process using a built-in computer based on the detected current value.
  • a robot control computer executes the collision determination process. ing. That is, the collision discrimination processing and the mouth bot control are executed by the same computer.
  • Robot control involves various types of complicated information processing, and therefore the computer control execution cycle and the collision judgment processing execution cycle must be relatively large. As a result, the collision judgment processing starts at the time of the collision. It takes time to complete, further increasing the time delay in detecting a collision.
  • a collision detection in a machine driven by a servomotor is performed.
  • the driving stop method is a step of periodically detecting a value of a collision determination parameter associated with a driving force of a servomotor.
  • A a step of periodically detecting a change in the collision discrimination parameter value
  • b a step of discriminating the occurrence of a collision when the change in the collision discrimination parameter value exceeds a discrimination reference value
  • the servo motor so that the motor rotation stops when it is determined that a collision has occurred.
  • D controlling the data.
  • the occurrence of a collision in the machine driven by the servomotor is detected and the rotation of the servomotor is stopped.
  • the occurrence of a collision can be quickly detected without using a special sensor, and the driving of the machine can be stopped quickly. As a result, damage to the machine due to the collision can be prevented or reduced.
  • FIG. 1 is a flow chart of a control program for implementing a driving stop method according to the first embodiment of the present invention.
  • FIG. 2 is a diagram similar to FIG. 1 for the second embodiment.
  • FIG. 3 is a view similar to FIG. 1 for the third embodiment,
  • FIG. 4 is a view similar to FIG. 1 for the fourth embodiment, and
  • FIG. 5 is a view similar to FIG. 5 for the fifth embodiment.
  • Fig. 1 is the same as Fig. 1
  • Fig. 6 is a schematic block diagram showing the main part of an industrial robot to which the method of the first embodiment is applied
  • Fig. 7 is one axis of the robot in Fig. 6.
  • Fig. 8 is a block diagram showing the control characteristics of the servo system related to Fig. 3 using a transfer function, and Fig. 8 shows the look-up chipper storing the collision determination reference value used in the method of the third embodiment.
  • FIG. 1 is a flow chart of a control program for implementing a driving stop
  • an industrial robot to which the method according to the first embodiment of the present invention is applied is basically configured as conventionally known, and includes an operation robot having various sensors (not shown).
  • various sensors not shown
  • the robot mechanism 13 has a series of links constituting an arm and an actuating section such as an end effector attached to the end of the arm.
  • Each of the actuating sections (one of them is indicated by reference numeral 13a) is
  • Each of the one or more shafts of each of the actuators is driven or rotated linearly by a corresponding actuator, each of which is preferably a speed detector 13. It consists of a servomotor with c attached (one of them is indicated by reference numeral 13b).
  • the control device has a main control unit 1 for robot control, and the main computer 1 is composed of a microprocessor, a ROM storing a control program executed by the microprocessor, a RAM used for temporary storage of data, and the like. Then, an operation command such as a movement command for each axis of the ti-bot is transmitted.
  • the axis controller 2 cooperates with the servo motor of each axis of the robot and the servo amplifier (one of which is indicated by reference numeral 12) which is a digital servo system of each axis, that is, a software servo.
  • the axis controller 2 is a digital control system with position control function 21, proportional control function 22, and integral control functions 23, 25 shown in Fig. 7. It has a signal processor (not shown) and sends a torque command corresponding to the movement command from the main computer 1 to a servo embed that produces the control characteristics 24 shown in Fig. 7 in cooperation with the servo motor. It has become.
  • the symbols Kp, ⁇ ⁇ ⁇ V and ⁇ 1 represent the g-loop gain, proportional gain and integral gain of the axis controller 2
  • the symbol Kt represents the torque constant of the servomotor
  • the symbol J represents the servomotor. Represents all the inertia applied to the output shaft.
  • the axis controller 2 calculates a collision determination reference value ERR, a flag information F indicating an operation mode in servo control of the digital signal processor and a speed deviation information ⁇ ⁇ , for collision determination, which will be described later. It has a memory (register) for storing.
  • control unit consists of the I / O unit 3 connected to the various sensors and actuators of the port and the various sensors of the work object 14, and the serial port connected to the teaching operation panel 7 and the operation panel 9. 4 and a data memory RA 5 backed up by the battery 8 and a power supply circuit 6 supplied from an external power supply (not shown) via the input transformer 11 and the input unit 10.
  • teaching operation panel 7 and the operation panel 9 are each provided with a display unit (not shown). Then, power is supplied from the power supply circuit 6 to the main computer 1 and the above-mentioned elements 2 to 5 connected thereto via the back panel 16 and the robot mechanism via the input unit 10. Power to parts 13 and Power is supplied to the servo amplifier 12 via the input unit 10 and the servo transformer 15.
  • the control equipment in Fig. 6 Prior to drying the robot, the control equipment in Fig. 6 is initialized, and the flag information F in the memory of the axis controller 2 is reset to "0". After that, when the operator operates the teaching operation panel 7 and the operation panel 9 to teach the robot as in the prior art, the teaching data is stored in the data memory RAM 5. Further, the operator operates the operation panel 9 to store the collision determination reference value ERR in the memory (register) of the axis controller 2. This value ERR is set to a value that is larger than the maximum change height of the speed deviation that can occur within the period corresponding to one cycle of the control operation of the axis controller 2 in the normal port pot operating state by a predetermined value. Is set.
  • the main computer 1 sequentially executes various control operations at a predetermined cycle.
  • This control operation includes, for example, a process of calculating a movement command for each axis of the robot in accordance with the teaching data stored in the data memory RAM 5.
  • the digital signal processor of the axis controller 2 has a smaller position for each axis of the robot in a cycle shorter than the control operation execution processing cycle in the main computer 1! ;, Speed and torque control operation (servo control operation) and collision judgment operation are performed. That is, the digital signal processor determines whether or not the flag F in the memory of the axis controller 2 has been reset to “0 J” in a certain control operation execution cycle (step S 1 in FIG. 1). Here, the flag F has been reset, and the result of the determination in step S 1 is negative (NO), so that the digital signal processor uses a servo motor, such as the servo motor, associated with the current control operation execution cycle.
  • a servo motor such as the servo motor
  • the output representing the actual motor rotation speed V from the speed detector 13 c attached to the motor 13 b is integrated as shown by reference numeral 25 in FIG. 7 to obtain the actual rotation position P of the servo motor 13 b. Then, the difference (position deviation) ⁇ ⁇ ′ between the command position P c indicated by the command signal related to the servomotor and the actual position gP f from the main computer 1 is calculated. calculate c, and The digital signal processor calculates the difference (speed deviation) ⁇ between the calculated command speed V c and the actual motor speed V as shown by reference numeral 26 in Fig. 7 (chip S 2).
  • step S 3 By subtracting the output speed deviation ⁇ ⁇ '(described later) in the previous servo control operation execution period from the value ⁇ V thus obtained, the change in the speed deviation within the period corresponding to one control period is obtained. It is determined whether or not the absolute value I ⁇ V — ⁇ V, I is smaller than the collision determination reference value ⁇ RR (step S 3).
  • the servo umbrella 12 supplies a drive current according to the torque command value Tqc to the servomotor 13 to rotate the servomotor at the speed V, thereby driving the robot operating portion 13a.
  • the digital signal processor stores the deviation Vc—V between the command velocity Vc and the actual velocity V in a register as the velocity deviation ⁇ v, used in step S3 for determining the collision in the next control cycle (step S7). Then, the processing in this control synchronization ends. Thereafter, the above-described processing is repeatedly executed at a predetermined control cycle.
  • the actual rotation speed V of the servo motor 13b decreases or the motor rotates. Leads to a stop.
  • the digital signal processor determines that a collision has occurred, sets the flag F to a value of “1”, and displays a message notifying the occurrence of the collision on, for example, a display unit of the operation panel 9 (schip).
  • the torque command value T qc is set to ⁇ 0 J (step S 9), and the process proceeds to stip S 6.
  • a torque command of the value “0 J” is sent from the axis controller 2 to the servo ambule 12, the drive current stops flowing to the servo motor 13, and the motor rotation stops.
  • the storage step S7 is further executed to end the processing in the current control cycle.
  • the method of this embodiment is different from the method of the first embodiment in which the collision is determined based on the change in the speed deviation, in that the collision is determined based on the change in the torque command. It should be noted that the method of this embodiment and the later-described embodiment Applies to the robot in the figure.
  • the digital signal processor of the axis controller 2 determines in each step of the servo control operation execution cycle that the flag F is not equal to the value ⁇ 1 J in step S100, and the steps in FIG.
  • the torque command value calculation chips S101 and S102 corresponding to the chips S4 and S5 are executed.
  • the torque command value T qc ′ in the previous servo control cycle is subtracted from the obtained torque command value T qc to obtain a change in the torque command value, and the absolute value IT qc—T qc, I It is determined whether or not it is smaller than the determination reference value ERR (Schip S103).
  • This value ERR is set to a value that is larger by a predetermined value than the maximum change in torque command that can occur within a period corresponding to one servo control operation execution cycle in a normal robot operation state. It is stored in a register in the axis controller 2 before operation. If the determination result in step S103 is affirmative, it is determined that the robot is in a normal robot operation state, and the torque command value calculation step S106 corresponding to step S6 is executed. The value T qc calculated in 102 is stored in a register as a value T qc, for the collision determination process (S 103) in the next control cycle (step S 107), and The process ends.
  • the actual rotation speed V of the servo motor 13b decreases. Or, the rotation of the motor stops.
  • the determination result in the collision determination step S103 is negative.
  • the digital signal processor executes steps S104 and S105 corresponding to the chips S8 and S9, respectively, to report the occurrence of the collision and stop the rotation of the servomotor. Then, the motor rotation stop state is maintained in the next and subsequent control periods.
  • FIGS. 3 and 8 a method of detecting and stopping a collision according to a third embodiment of the present invention will be described.
  • This embodiment is basically the same as the first embodiment. However, the point at which the collision determination reference value is variably set according to the command speed and the point at which the magnitude of the actual motor speed is proportional to the magnitude of the collision and a torque command that acts in the opposite direction to the actual motor rotation direction when a collision occurs are sent. This is different from the first embodiment.
  • the memory of the axis controller 2 includes a lookup chip Tab in which collision determination reference values corresponding to each of the N + 1 command speed sections are stored.
  • the value ERR 0 is the collision determination at the start of the servo motor drive.
  • V c max represents the maximum command speed
  • N is an integer value obtained by subtracting “ ⁇ ” from the number of command speed sections.
  • ERR i is set so that the relationship of ERR i and ERR i + 1 is established, and the larger the command speed is, the larger the speed deviation change is allowed. In other words, taking into account that the higher the motor rotation speed at the time of collision, the smaller the change in motor rotation speed is, the more quickly the collision occurrence is detected while preventing erroneous detection of collision occurrence discrimination. Make it possible.
  • the digital signal processor of the axis controller 2 corresponds to steps S1 to S5 in FIG. 1, respectively, and is related to collision determination, torque command value calculation, and the like.
  • Steps S 200 to S 204 are sequentially executed, and the resulting torque command value T qc is sent to the servo amplifier 12.
  • step S202 If the determination result in the collision determination step S202 is negative during the robot operation, the digital signal processor sets the flag F to the value "1" in step 207 corresponding to step S8 and sets the alarm display. Do. As described above, the collision determination reference value E RR used in step S202 corresponds to the maximum allowable motor speed reduction change that changes depending on the command speed, and therefore, the collision determination is performed accurately and quickly.
  • step S208 the command speed value Vc and the accumulator value A are set to "0", respectively, and then the process proceeds to step S204 to output the torque command TqC. This output torque command Tqc is expressed by the following equation (2).
  • T qc — V * K v ⁇ » ⁇ (2)
  • the digital signal processor The difference storage / collision determination reference value selection steps S205 and S206 are executed to end the processing in the current control cycle, and in the next and subsequent control cycles, the torque shown in equation (2) is obtained.
  • the torque command value T qc having a magnitude that is actually proportional to the servo motor speed V and acting in the direction opposite to the actual motor rotation direction is sent to the servo ambu 12.
  • the servo motor 13b is rapidly and smoothly braked, stops rotating in a short time, and stops driving the robot operating unit 13a.
  • the torque command value T qc decreases as the actual motor speed decreases after the collision.
  • This embodiment is basically the same as the second practical example.
  • the point at which the magnitude of the collision determination reference value is changed according to the command speed, and the point at which the torque command is transmitted when the magnitude is proportional to the actual motor speed and acts in the opposite direction to the actual motor rotation direction when a collision occurs are different from the second embodiment, and are similar to the third embodiment in these points.
  • a collision determination reference value corresponding to each of the N + 1 command speed sections and having a larger value as the command speed increases is stored. Use one pull (not shown).
  • the collision determination reference value in the present embodiment represents the allowable change in the torque command within a period corresponding to one control cycle, and the allowable change in the speed deviation.
  • the magnitude is different from the collision determination reference value in FIG. 8 representing the height.
  • the digital signal processor corresponds to the steps S100 to S103, S106, and S107 in FIG. S300 to S303, S30 and S308 for checking the operation mode, calculating the torque command value, determining the collision and storing the torque command value, and colliding with step S206 in FIG. Step S309 relating to the selection of the determination reference value is executed in this order.
  • step S303 If a collision occurs during the robot operation, the result of the determination in the collision determination step S303 corresponding to step S103 in FIG. 2 is negative, and the digital signal processor returns to step S103 in FIG.
  • steps S305 and S204 relating to the calculation of the torque command value upon occurrence of a collision correspond to steps S208 and S204 in FIG. 3, respectively.
  • the step S306 is executed, and the steps S307 to S309 described above are executed.
  • This embodiment is basically the same as the first embodiment. You. However, the point that the magnitude of the collision determination reference value is changed according to the type of operation command read from the program by the control device in FIG. 6 and the excessive error alarm function of the control device in FIG. 6 when a collision occurs Is different from the first embodiment.
  • a look-up chip (not shown) storing a required number of collision determination reference values similar to the chiple Tab of FIG. 8 is used. Then, the collision discrimination reference value is set in association with the type of the operation command, for example, G01 code representing linear interpolation, G02 code representing circular interpolation, and the like, and the motion command being executed is set. Optimal collision discrimination can be performed in accordance with the operating state of the working part that changes depending on the type. In addition, in the present embodiment, the reason for disabling the excessive error alarm function at the time of collision determination is that the control parameter in the servo control system is too large (ie, the difference between the command position and the actual position).
  • Excessive error alarm function that acts to release the excitation of the servo motor when it becomes active can be activated in the event of a collision in which the deviation increases, in which case the servo motor is operated under the control of the axis controller 2. This is because it becomes impossible to perform the method of the present invention for performing the rotation stop control of the servo motor after the collision is detected, which may be an obstacle.
  • step S501 prior to the collision determination step S503
  • the collision determination reference value ERR corresponding to the type of the operation command related to the current control cycle is read from the memory.
  • step S508 the excessive error alarm function of the robot control device is invalidated in step S508, and the servo motor is operated under the control of the axis controller 2 even after the collision occurs.
  • steps S509, S510, S506, and S507 corresponding to steps S8, S9, S6, and S7 in FIG. Step S l,
  • Schips S500 S510, S506 and S507 corresponding to S9, S6 and S7, respectively, are executed.
  • the collision discrimination parameter is: a collision discrimination reference value (constant or variable according to the type of command speed or operation command), a torque command value to be transmitted after the collision discrimination. (0 or 1 KVJ) and a case where the invalidation processing of the excessive error alarm function is combined in a specific mode has been described, but another combination is also possible.
  • the process of disabling the excessive error alarm function may be applied to the first to fourth embodiments, and in the second embodiment, as in the fifth embodiment, an operation command is given to the collision determination reference value.
  • the collision determination reference value corresponding to the command speed is obtained from the look-up table.
  • the required collision determination value is determined experimentally in advance and based on an expression that is a function of the command speed.
  • the determination reference value may be calculated.
  • the collision determination reference value is fixed or variable according to the command speed or the type of the operation command.
  • the collision determination reference value is determined according to the operation position g of the machine operating unit. Variable setting may be used. In this case, for example, it represents a series of blocks of a control program of various machines, and therefore stores a required number of collision determination reference values in a lookup table in association with each of the sequence numbers representing the operating position g area. deep.
  • the product of the maximum change in speed deviation or the maximum change in torque command value and the safety coefficient within a period corresponding to one servo control cycle may be used as the collision determination reference value.
  • the speed deviation or the change in torque command value from the previous detection time to the current detection time in each detection cycle is sequentially determined. Calculate and store, and find the maximum value of the change height after the operation is completed. In this case, the maximum value of the change height may be obtained for each type of operation command or for each operation position area of the mechanical operation unit.
  • the absolute value of the difference between the previous speed deviation ⁇ ⁇ 'and the current speed deviation ⁇ V or the previous torque command value T qc' and the current torque command value T qc Sum of the absolute value of the difference between and the predetermined value B I ⁇ V— ⁇ V, I + B or IT qc — T qc, ⁇ + ⁇ may be calculated, and this added value may be used as the collision determination reference value in the next control cycle.
  • the value is proportional to the torque command with a value of “0” or the actual motor speed after the occurrence of the collision, and acts in the direction opposite to the actual motor rotation direction. Since the torque command is sent out, the motor rotation can be stopped quickly and smoothly in the event of a collision, minimizing damage to machine parts due to the collision. Wear.
  • the collision determination reference value is variably set in accordance with the magnitude of the command speed, the type of operation of the machine operating unit, etc., so that even when various machines are operating in any operating state, they can be quickly operated. A collision can be detected, and no false detection occurs.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Numerical Control (AREA)
  • Control Of Electric Motors In General (AREA)
  • Safety Devices In Control Systems (AREA)
  • Control Of Position Or Direction (AREA)

Description

明 細 害
サーボモータによ り駆動される機械における
衝突検出 ♦ 駆動停止方法
技 術 分 野
本発明は、 サーボモータによ り駆動される機械作動部 と異物等との衝突を迅速に検出可能でかつ衝突検出時に 機械作動部の駆動を迅速に停止可能な衝突検出 ♦ 駆動停 止方法に関する。
背 景 技 術
機械の運転時、 機械作動部とその作動領域に入り込ん だ異物とが干渉し、 機械の正常運転状態が阻害されある いは機械が破損するに至ることがある。 従来、 種々の方 法でこれを防止している。
たとえば、 機械作動部の外面に取り付けたタツチセン サを用いて機械作動部と異物との衝突を検出している。 しかし、 センサの設置に伴って機械がコス ト髙になり、 しかも、 センサの作動領域外における機械作動部と異物 との衝突を検出できない。 また、 機械作動部をサーボモ 一夕で駆動するタイブの機械において、 機械作動部と異 物との衝突に伴いサーポモータ駆動電流が所定の判別値 を越えたとき両者の衝突を検出することが知られている。 しかしながら、 衝突時点からサーボモータ駆動電流が実 際に当該判別値に到達するまでに時間を要するので、 衝 突検出上、 時間的な遅れが生じ、 衝突による被害が大き くなる。 さらに、 センサ出力あるいはサーボモータ駆動 電流の検出値に基づいて内蔵のコンピュータで衝突判別 処理を行うタイブの機械たとえば産業用ロポッ トが知ら れ、 この種の機械は、 衝突判別処理をロボッ ト制御用コ ンビュータで実行するようになっている。 すなわち、 衝 突判別処理と口ボッ ト制御とを同一のコンピュータで実 行している。 ロボッ ト制御は各種の複雑な情報処理を含 み、 従って、 コンピュータの制御実行周期および衝突判 別処理実行周期は比較的大きいものにならざるを得ず、 結果として、 衝突時点から衝突判別処理が完了するまで に時間を要し、 衝突検出上の時間的な遅れがさらに増大 する。
発 明 の 開 示
本発明の目的は、 サーボモータにより駆動される機械 作動部と異物等との衝突を迅速に検出可能かつ衝突検出 時に檨械作動部の駆動を迅速に停止可能で、 衝突による 機械の破損等を防止もし くは軽減可能な衝突検出 ♦ 駆動 停止方法を提供することにある。
上述の目的を達成するため、 本発明の、 サーボモータ により駆動される機械における衝突検出 ♦ 駆動停止方法 は、 サーポモータの駆動力に閬連する衝突判別パラメ一 タの値を周期的に検出する工程 （ a ) と、 前記衝突判別 パラメータ値の変化高を周期的に検出する工程 （ b ) と、 前記衝突判別パラメータ値の変化高が判別基準値を越え たとき衝突発生を判別する工程 （ c ) と、 衝突発生を判 別したときモータ回転が停止するように前記サーボモー タを制御する工程 （ d ) とを備える。
本発明によれば、 衝突判別パラメータ値の変化高が判 別基準値を越えたとき、 サーボモータによ り駆動される 機械における衝突発生を検出しかつサーボモータの回転 を停止させるようにしたので、 特別のセンサを用いずに 衝突発生を迅速に検出しかつ機械の駆動を迅速に停止で き、 結果として衝突による機械の破損等を防止もし くは 軽減できる。 ''
図 面 の 簡 単 な 説 明
第 1 図は本発明の第 1 の実施例による衝突検出 ♦ 駆動 停止方法を実施するための制御ブログラムのフローチヤ ー ト、 第 2図は第 2の実施例についての第 1 図と同様の 図、 第 3図は第 3の実施例についての第 1 図と同様の図、 第 4図は第 4の実施例についての第 1 図と同様の図、 第 5図は第 5の実施例についての第 1 図と同様の図、 第 6 図は第 1 の実施例の方法が適用される産業用ロボッ トの 要部を示す概略ブロック図、 第 7図は第 6図のロボッ ト のひとつの軸に閬連するサーボ系の制御特性を伝達関数 を用いて示したブロック線図、 および、 第 8図は第 3の 実施例の方法で用いられる衝突判別基準値を格納したル ックアップチープルを示す図である。
発明を実施するための最良の形態
第 6図を参照すると、 本発明の第 1 の実施例の方法が 適用される産業用ロボッ トは、 基本的には従来公知のよ うに構成され、 各種センサ （図示省略） を有する作業対 象 1 4に対して所要の作業を行うためのロボッ ト機構部 1 3と、 該ロボッ ト機構部 1 3を制御するための、 後述 の各種要素よりなる制御装置と、 各種センサ （図示省略） とを備えている。
ロボッ ト機構部 1 3は、 アームを構成する一連のリン クおよびアーム先端に装着したエンドエフヱクタ等の作 動部を有し、 作動部の各々 （その内のひとつを符号 1 3 aで示す） は、 各該作動部の一つ以上の軸の各々に関し てこれに対応するァクチユエータにより直線的に駆動ざ れあるいは回転駆動されるようになっており、 夫々のァ クチユエータは好ましく は速度検出器 1 3 cを付設した サーボモータ （その内のひとつを符号 1 3 bで示す） よ り成る。
制御装置はロボッ ト制御甩の主コンビュ一夕 1 を備え、 主コンピュータ 1 は、 マイクロプロセッサ， 該マイクロ プロセッサが実行する制御プログラムを格納した R O M, データの一時記憶等に利用される R A M等により構成さ れ、 tiボッ トの各軸についての移動指令などの動作指令 を送出するようになっている。
軸制御器 2は、 ロボッ トの各軸のサーボモータおよび これに鬨連するサーボアンプ （その内のひとつを符号 1 2で示す〉 と協働して各軸のデジタルサーボ系すなわち ソフ ト ウェアサーボ系を構成している。 すなわち、 軸制 御器 2は、 第 7図に示す位置制御樣能 2 1， 比例制御機 能 2 2および積分制御機能 2 3， 2 5を備えるデジタル シグナルプロセッサ （図示省略） を有し、 サーボモータ と協働して第 7図に示す制御特性 2 4を現出させるサー ボアンブに、 主コンピュータ 1 からの移動指令に応じた トルク指令を送出するようになっている。 第 7図におい て、 符号 K p， Κ Vおよび Κ 1 は軸制御器 2の位 gルー ブゲイ ン， 比例ゲインおよび積分ゲインを、 符号 K t は サーボモータの トルク定数を、 符号 J はサーボモータの 出力軸に加わる全イナ一シャをそれぞれ表す。 そして、 軸制御器 2は、 後で詳述する衝突判別基準値 E R R , デ ジタルシグナルプロセッサのサーボ制御上の動作モー ド を表すフラグ情報 Fおよび衝突判別のための速度偏差情 報 ε ν ， を記憶するためのメモリ （レジスタ〉 を有して いる。
また、 制御装置は、 Πポッ トの各種センサおよび各種 ァクチユエータならびに作業対象 1 4の各種センサに接 続された I / 0ユニッ ト 3と、 教示操作盤 7および操作 パネル 9に接続されたシリアルポー ト 4と、 ) ッテリ 8 によりバックアップされたデータメモリ用 R A Μ 5と、 入力ト ランス 1 1 および入力ユニッ ト 1 0を介して外部 電源 （図示略） から辁電される電源回路 6 とを有し、 教 示操作盤 7および操作パネル 9には表示部 （図示略） が 夫々設けられている。 そして、 電源回路 6から主コンビ ユータ 1 およびこれに接続された上述の要素 2〜 5への 耠電をバックパネル 1 6を介して行うと共に、 入力ュニ ッ ト 1 0を介してロボッ ト機構部 1 3に耠電し、 また、 入力ユニッ ト 1 0およびサーボト ランス 1 5を介してサ 一ボアンブ 1 2に辁電するようにしている。
以下、 第 1図を参照して、 上述のように構成したロボ ッ トの動作を説明する。
ロボッ トの運乾に先立ち、 第 6図の制御装匱が初期化 され、 軸制御器 2のメモリ内のフラグ情報 Fが 「 0」 に リセッ ト される。 その後、 オペレータが教示操作盤 7お よび操作パネル 9を操作してロボッ トへの教示を従来公 知のように行う と、 教示データがデータメモリ用 R A M 5に格納される。 又、 オペレータは、 操作パネル 9を操 作して衝突判別基準値 E R Rを軸制御器 2のメモリ （レ ジスタ） に格納する。 この値 E R Rは、 正常な口ポッ ト 運転状態において、 軸制御器 2の 1サ-ポ制御動作実行 周期に対応する期間内に発生し得る速度偏差の最大変化 高よりも所定値だけ大きい値に設定される。
ロボッ トの運転時、 主コンピュータ 1 は所定の周期で 各種制御動作を順次実行する。 この制御動作には、 例え ば、 データメモリ用 R A M 5に格納された教示データに 従ってロボッ トの各軸に対する移動指令を演算する処理 が含まれる。
軸制御器 2のデジタルシグナルブロセッサは、 主コン ビュータ 1 における制御動作実行処理周期よ り短い周期 でロボッ トの各軸についての位！;， 速度およびトルク制 御動作 （サーボ制御動作） ならびに衝突判別動作を実行 する。 すなわち、 デジタルシグナルプロセッサは、 或る制御 動作実行周期において、 軸制御器 2のメモリ内のフラグ Fが 「 0 J にリセッ ト されているか否かを判別する （第 1図のステップ S 1 ) 。 ここではフラグ Fがリセッ ト さ れており、 ステップ S 1 の判別結果が否定 （ N O) とな る。 そこで、 デジタルシグナルブロセッサは、 今回の制 御動作実行周期に閬連するサーボモータたとえばサーボ モータ 1 3 bに装着した速度検出器 1 3 cからの実際モ ータ回転速度 Vを表す出力を、 第 7図に符号 25で示す ように積分してサーボモータ 1 3 bの実際回転位置 P f を算出する。 次いで、 主コンピュータ 1からの、 当該サ ーボモータに関連する指令信号が示す指令位置 P c と実 際位 gP f との差 （位箧偏差） ε ρ'に応じた指令速度 V cを算出する。 そして、 デジタルシグナルプロセッサは、 第 7図に符号 2 6で示すように、 算出指令速度 V c と実 際モータ速度 Vとの差 （速度偏差） ε νを算出する （ス チップ S 2) 。 次に、 斯く箅出した値 ε Vから前回のサ -ボ制御動作実行周期での箅出速度偏差 ε ν ' (後述） を減じて得た 1制御周期に対応する期間内での速度偏差 の変化高の絶対値 I ε V — ε V， I が衝突判別基準値 Ε R Rよ り小さいか否かを判別する （ステップ S 3 )。
スチップ S 3での判別結果が肯定 （ Y E S ) であれば、 デジタルシグナルプロセッサは正常なロボッ ト運転状態 にあると判別して通常の制御動作を実行する。 即ち、 第 7図に符号 23で示すように、 速度偏差 ε ν ( = V c - V ) と積分ゲイン K 1 との積値を箅出し、 当該積値 K 1 ♦ ( V c - V ) をアキュムレータの値 Αに加算する （ス チップ S 4〉 。 次に、 第 7図に符号 2 2で示すように、 速度偏差 ε ν ( = V c - V ) と比例ゲイン Κ νとの積値 を算出し、 当該箅出値 Κ ν * ( V c - V ) をアキュムレ 一タの値 Αに加算してトルク指令値 T q c を求め、 この 値 T q c を D Z A変換してサーボアンブ 1 2に出力する (ステップ S 5， S 6 ) 0
サーボアンブ 1 2は、 この トルク指令値 T q cに応じ た駆動電流をサーボモータ 1 3 に流して該サーボモー タを速度 Vで回転させ、 ロボッ ト作動部 1 3 aを駆動す る。 デジタルシグナルプロセッサは、 指令速度 V c と実 際速度 Vとの偏差 V c— Vを次の制御周期での衝突判別 ステップ S 3で用いる速度偏差 ε v， としてレジスタに 格納し （ステップ S 7 ) 、 今回の制御同期での処理を終 了する。 その後、 上述の処理を所定の制御周期で繰り返 し実行する。
上記サーボ制御に従うロボッ ト逞耘中、 ロボッ ト作動 部 1 3 aが例えば該作動部の作動領域内に入り込んだ異 物と衝突すると、 サーポモータ 1 3 bの実際回転速度 V が低下しまたはモータ回転が停止するに至る。 斯かる場 合、 衝突発生直後のサーボ制御動作実行周期でのスチッ プ S 2において算出される速度偏差 ε V ( = V c - V ) が急増し、 当該制御周期のステップ S 3において、 前回 の衝突判別時点から今回判別時点までの期間内での速度 偏差の変化高の絶対値 I ε V — ε V， I が衝突判別基準 値 E R Rよ りも大きいと判別される。 この場合、 デジ夕 ルシグナルプロセッサは、 衝突発生と判別してフラグ F を値 「 1 」 にセッ トすると共に、 衝突発生を告知するメ ッセージを例えば操作パネル 9の表示部に表示する （ス チップ S 8 ) 。 次に、 トルク指令値 T q c を Γ 0 J に設 定し （ステップ S 9 ) 、 スチップ S 6に移行する。 この 結果、 軸制御器 2からサーボアンブ 1 2に値 「 0 J の ト ルク指令が送出され、 サーボモータ 1 3 に駆動電流が 流れなくなり、 モータ回転が停止する。 デジタルシグナ ルプロセッサは、 上記速度偏差格納ステップ S 7をさら に実行して今回制御周期での処理を終了する。
今回制御周期のスチップ S 8でフラグ Fが値 「 1 」 に セッ ト されたことから、 次回およびそれ以降のサーボ制 御周期ではステップ S l， S 9 , S 6および S 7よ りな る'処理が繰り返し実行され、 モータ回転停止状態が維持 される。
.以上において、 ひとつのロボッ ト制御軸に関して作動 説明したが、 その他の軸についても同様である。
次に、 第 2図を参照して、 本発明の第 2の実施例によ る衝突検出 ♦ 停止方法を説明する。
この実施例の方法は、 速度偏差の変化高に基づいて衝 突判別を行う上記第 1 の実施例の方法と比較して トルク 指令の変化高に基づいて衝突判別を行う点が相違する。 なお、 本実施例および後述の実施例の方法は例えば第 6 図のロボッ トに適用される。
正常なロボッ ト運転時、 軸制御器 2のデジタルシグナ ルプロセッサは、 サーボ制御動作実行周期の各々におい てステップ S 1 0 0でフラグ Fが値 Γ 1 J でないと判別 し、 第 1 図のステップ S 2の場合と同様の処理によって 速度偏差 ε V ( = V c— V ) を求める。 次いで、 スチッ ブ S 4， S 5に夫々対応する トルク指令値算出スチップ S 1 0 1 , S 1 0 2を実行する。 そして、 この結果得た ドルク指令値 T q cから前回のサーボ制御周期での トル ク指令値 T q c ' を減じてトルク指令値の変化高を求め、 その絶対値 I T q c— T q c， I が衝突判別基準値 E R Rよりも小さいか否かを判別する （スチップ S 1 0 3 )。 この値 E R Rは、 正常なロボッ ト運転状態において 1サ ーボ制御動作実行周期に対応する期間内に発生し得る ト ルク指令の最大変化高よ りも所定値だけ大きい値に設定 され、 ロボヅ ト運転前に軸制御器 2内のレジスタに予め 格納されている。 ステップ S 1 0 3での判別結果が肯定 ならば、 正常なロボッ ト運転状態にあると判別してステ ッブ S 6に対応する トルク指令値算出ステッブ S 1 0 6 を実行した後、 ステップ S 1 0 2で算出した値 T q cを、 次の制御周期での衝突判別処理 （ S 1 0 3 ) のための値 T q c ， としてレジスタに格納し （ステップ S 1 0 7 )、 今回周期での処理を終了する。
ロボッ ト運転中、 ロボッ ト作動部 1 3 aが異物と衝突 すると、 サーボモータ 1 3 bの実際回転速度 Vが低下し またはモータ回転が停止するに至る。 斯かる場合、 衝突 直後の制御周期で箅出される速度偏差 ε V ( = V c - V ) が急増し、 従って、 当該制御周期のステップ S 1 0 2に おいて箅出される トルク指令値 T q cが急増する。 この 結果、 衝突判別ステップ S 1 0 3の判別結果が否定にな る。 デジタルシグナルプロセッサは、 スチップ S 8およ び S 9に夫々対応するステップ S 1 0 4および S 1 0 5 を実行して衝突発生を聱告すると共にサーボモータの回 転を停止させる。 そして、 次回およびそれ以降の制御周 期においてモータ回転停止状態を維持する。
次に、 第 3図および第 8図を参照して、 本発明の第 3 の実施例による衝突検出 ♦ 停止方法を説明する。
この実施例は基本的には上記第 1 の実施例と同一であ る。 但し、 衝突判別基準値を指令速度に応じて可変設定 する点と、 衝突発生時に実際モータ速度と大きさが比例 しかつ実際モータ回転方向と逆方向に作用する トルク指 令を送出する点とが第 1 の実施例と異なる。
このため、 軸制御器 2のメモリは、 N + 1個の指令速 度区間の夫々に対応する衝突判別基準値が格納されたル ックアップチープル T a bを備えている。 第 8図に示す ように、 テーブル T a bは N + 1 個のア ド レス I ( = 0， 1， ♦ ♦ ♦ , N ) を備え、 第 0番目のア ド レスは指令速 度 V c が 「 0 J である第 0番目の指令速度区間に対応し、 当該ァ ド レスには衝突判別基準値 E R R oが格納されて いる。 値 E R R 0はサーボモータ駆動開始時の衝突判別 に用いられる。 そして、 第 i番目 （ i = 1， 2 , · ♦ ♦， N ) のア ド レスは、 指令速度 V cの絶対値が （ i 一 1 ) ♦ V c max/ Nよ りも大きくかつ i * V c max/ N以下の 値をとる第 i番目の区間に対応し、 このア ドレスには基 準値 E R R iが格納されている。 こ こで、 V c maxは最大 指令速度を表し、 Nは指令速度区間の数から Γ ΐ 」 を減 じた整数値である。 値 E R R iは、 E R R iく E R R i + 1 なる関係が成立するように設定され、 指令速度が大きい ほど大きな速度偏差変化を許容するようにしている。 換 言すれば、 衝突時のモータ回転速度が大きいほどモータ 回転速度の婊少変化高が大きい点を考慮して、 衝突発生 判別上の誤検出を防止しつつ衝突発生を可能な限り迅速 に検出可能なようにしている。
第 3図を参照すると、 ロボッ ト運転時、 軸制御器 2の デジタルシグナルブロセッサは、 第 1図のステップ S 1 〜 S 5に夫々対応しかつ衝突判別， トルク指令値算出な どに関連するスチッブ S 200〜 S 204を順次実行し、 その結果得たトルク指令値 T q cをサーポアンプ 1 2に 送出する。 そして、 ステップ S 7に対応する速度偏差格 納ステップ S 20 5を実行した後、 指令速度 V cが N + 1個の指令速度区間のいずれに属するかを判別する （ス チヅブ S 206 ) 。 すなわち、 デジタルシグナルブロセ ッサは、 指令速度 V cを演算定数 K ( = V c max/N) で除して値 V c /Kを求め、 次いで、 箅出値 V c /Kの 小数 1位以下の端数を切上げて整数を求め、 さらに、 そ の絶対値 （以下、 1 ^[丁 1 ¥ (：ノ 1{ | とぃぅ） を算出す る。 値 I N T I V c / K I は、 指令速度 V cが属する指 令速度区間に対応するルックアップチープルのァ ドレス I ( 0， 1 , ♦ ♦ ♦， N ) を表す。 即ち、 下記第 （ 1 〉 式が成立する。
I = I N T I V c / K I
次に、 デジタルシグナルプロセッサは、 ルックァッブ テーブル T a bの第 I番目のァ ド レスから所要の衝突判 別基準値 E R R i ( i = 0， 1 ， N) を読出し、 この判別基準値を軸制御器 2の所定のレジスタに格納す る （ステップ S 2 06 ) 。
ロボッ ト運転中、 衝突判別ステップ S 2 02での判別 結果が否定であると、 デジタルシグナルプロセッサは、 ステップ S 8に対応するステップ 207においてフラグ Fを値 「 1」 にセッ トすると共にアラーム表示を行う。 上述のようにステップ S 202で用いられる衝突判別基 準値 E R Rは指令速度に依存して変化する最大許容モー タ速度減少変化高に対応し、 従って、 衝突判別は的確か つ迅速に行われる。 次いで、 ステップ S 2 08において 指令速度値 V cおよびアキュムレータの値 Aを夫々 「 0」 にセッ ト した後、 上述のステップ S 204に移行して ト ルク指令 T q C を箅出する。 この箅出トルク指令 T q c は下記第 （ 2) 式で表される。
T q c =— V * K v ♦ » ♦ ( 2 ) 次に、 デジタルシグナルプロセッサは、 上述の速度偏 差格納/衝突判別基準値選択ステップ S 2 0 5 , S 2 0 6を実行して今回の制御周期での処理を終了し、 次回及 びそれ以降の制御周期において第 （ 2 ) 式に示すトルク 指令値 T q cを送出する。 この様に、 衝突判別後は、 実 際サーボモータ速度 Vに比例する大きさを有しかつ実際 モータ回転方向と逆方向に作用する トルク指令値 T q c がサーボアンブ 1 2に送出される。 この結果、 サーボモ ータ 1 3 bが急激にかつ円滑に制動されて短時間内に回 転停止し、 ロボッ ト作動部 1 3 aが駆動停止する。 なお、 衝突発生以降の実際モータ速度の減少に伴って トルク指 令値 T q cは潮減する。
" 次に、 第 4図を参照して、 第 4の実施例による衝突検 出 ♦停止方法を説明する。
この実施例は基本的には上記第 2の実旛例と同一であ る。 但し、 衝突判別基準値の大きさを指令速度に応じて 変化させる点と、 衝突発生時に実際モータ速度と大きさ が比例しかつ実際モータ回転方向と逆方向に作用する ト ルク指令を送出する点とが第 2の実施例と異なり、 また、 これらの点で第 3の実施例に類似している。 このため、 第 8図に示すルックアップチープル T a b に類似の、 N + 1個の指令速度区間の夫々に対応しかつ指令速度 大 きいほど大きい値をとる衝突判別基準値が格納されたチ 一プル （図示略） を用いる。 但し、 本実施例における衝 突判別基準値は、 1制御周期に対応する期間内での トル ク指令の許容変化高を表すもので、 速度偏差の許容変化 高を表す第 8図の衝突判別基準値とは大きさが異なる。 第 4図を参照すると、 正常なロボッ ト運転時、 デジタ ルシグナルブロセッサは、 第 2図のスチッブ S 1 00〜 S 1 0 3, S 1 0 6および S 1 0 7に夫々対応しプロセ ッサ動作モー ドの確認， トルク指令値の算出， 衝突判別 および トルク指令値の格納に係るスチップ S 300〜 S 303, S 30マおよび S 308と、 第 3図のステップ S 20 6に対応し衝突判別基準値の選択に係るステップ S 30 9をこの順序で実行する。
ロボッ ト運転中に衝突が発生すると、 第 2図のステツ ブ S 1 0 3に対応する衝突判別ステップ S 303での判 別結果が否定になり、 デジタルシグナルブロセッサは、 第 2図のステップ S 1 04に対応するプロセッサ動作モ 一 ド切換 アラーム表示ステップ S 304を実行した後、 第 3図のステップ S 20 8および S 204に夫々対応し 衝突発生時の トルク指令値算出に係るステップ S 305 および S 306を実行し、 さらに、 上述のステップ S 3 07〜 S 309を実行する。 この結果、 第 3の実旅例の 場合と同様、 衝突判別後は、 実際サーボモータ速度 Vの 大きさに比例する大きさを有しかつ実際モータ回転方向 と逆方向に作用する トルク指令値 T q cに従ってサーポ モータ 1 3 bが急制動ざれる。
次に、 第 5図を参照して、 第 5の実施例による衝突検 出 ♦ 停止方法を説明する。
この実施例は基本的には上記第 1の実施例と同一であ る。 但し、 第 6図の制御装置によ りプログラムから読み 出される動作指令の種別に応じて衝突判別基準値の大き さを変化させる点と、 衝突発生時に第 6図の制御装置の 誤差過大ァラーム機能を無効にするようにした点とが第 1 の実施例と異なる。
このため、 本実施例では、 第 8図のチープル T a bに 類似の、 所要数の衝突判別基準値が格納されたルックァ ップチ一プル （図示略） を用いている。 そして、 動作指 令の種別、 例えば、 直線補間を表す G 0 1 コー ド， 円弧 補間を表す G 0 2コー ド等の夫々に対応づけて衝突判別 基準値を設定し、 実行中の動作指令の種別に依存して変 化する棲械作動部の動作状態に即した最適な衝突判別を 行えるようにしている。 また、 本実施例において衝突判 別時に誤差過大アラーム檨能を無効にする理由は、 サー ポ制御系での制御パラメータである位 g傭差 （指令位置 と実際位箧との差〉 が過大になったときにサーポモータ の励磁を解く ように作用する誤差過大アラーム檨能が位 g偏差が増大する衝突発生時に作動可能であり、 斯かる 場合にはサーボモータを軸制御器 2の制御下で作動させ ることができなくなり、 衝突検出以降にサーポモータの 回転停止制御を行う本発明の方法を実施する上で障害と なり得るからである。
本実施例が適用されるロボッ トの作動は第 1， 第 3の 実施例の場合と略同様で、 ロボッ ト正常運転時、 第 1図 のステップ S 1 〜 S 7に夫々対応するスチップ S 5 0 0 , S 5 0 2〜 S 5 0 7が実行される。 但し、 衝突判別ステ ッブ S 5 0 3に先立つステップ S 5 0 1 において、 今回 制御周期に関連する動作指令の種別に対応する衝突判別 基準値 E R Rをメモリから読み出す点が異なる。 そして、 衝突発生時にはステップ S 5 0 8においてロボッ ト制御 装置の誤差過大アラーム機能を無効にし、 衝突発生後に おいてもサーボモータを軸制御器 2の制御下で作動させ る。 また、 衝突発生直後は第 1 図のステップ S 8, S 9. S 6及び S 7に夫々対応するステップ S 5 0 9， S 5 1 0， S 5 0 6及び S 5 0 7を、 その後はステップ S l，
S 9, S 6および S 7に夫々対応するスチップ S 5 0 0: S 5 1 0, S 5 0 6および S 5 0 7を実行する。
本発明は上述の第 1 〜第 5の実施例に限定されるもの ではなく、 ロボッ ト以外の各種機械に適用可能であると 共に種々の変形が可能である。 例えば、 上記各実施例で は、 衝突判別パラメータ （速度偏差又は トルク指令値） ： 衝突判別基準値 （一定或は指令速度または動作指令の種 別に応じて可変） ， 衝突判別後に送出する トルク指令値 ( 「 0」 または 「一 K V J ) および誤差過大アラーム機 能の無効化処理を特定の態様で組合せた場合を説明した が、 別の紐合せも可能である。 例えば、 第 5の実施例に 示す誤差過大アラ -ム機能の無効化処理を第 1 〜第 4の 実施例に適用しても良い。 また、 第 2の実施例において- 第 5の実施例と同様、 衝突判別基準値を動作指令の種別 に応じて可変設定しても良い。 第 3， 第 4の実施例ではルックアップテーブルから指 令速度に応じた衝突判別基準値を求めたが、 予め実験的 に決定されかつ指令速度の関数である演箅式に基づいて 所要の衝突判別基準値を箅出しても良い。 また、 上記各 実施例では、 一定の或は指令速度または動作指令の種別 に応じて可変の衞突判別基準値を用いたが、 衝突判別基 準値を機械作動部の動作位 gに応じて可変設定にしても 良い。 この場合、 例えば、 各種機械の制御プログラムの 一連のブロックを表し、 従って、 動作位 g領域を表すシ 一ケンス番号の夫々に対応づけて所要数の衝突判別基準 値をルックアップテーブルに格納しておく。
さらに、 1サーボ制御周期に対応する期間内での速度 偏差の最大変化高または トルク指令値の最大変化高と安 全係数との積値を衝突判別基準値として用いても良い。 この場合、 機械を正常運転させつつ周期的に検出した速 度偏差またはトルク指令値に基づいて、 夫々の検出周期 における前回検出時点から今回検出時点までの速度偏差 または トルク指令値の変化高を順次算出しかつ記憶し、 運転終了後に変化高の最大値を求める。 この場合、 変化 高の最大値を動作指令の種別毎に又は檨械作動部の動作 位置領域毎に求めても良い。
また、 各々のサーボ制御実行周期において、 前回の速 度偏差 ε ν ' と今回の速度偏差 ε Vとの差の絶対値また は前回の トルク指令値 T q c ' と今回の トルク指令値 T q c との差の絶対値と所定値 Bとの和 I ε V— ε V， I + Bまたは I T q c — T q c ， Ι + Βを算出し、 この加 算値を次の制御周期で衝突判別基準値と して用いても良 い。
第 4の実施例においては トルク指令値 T q cを算出式 T q c = A + ( V c— V ) * K vに従うルーチン化した 演箅処理によ り求める点に関連して衝突検出後の トルク 指令値をこの算,出式に V C = 0， A= 0を代入して求め るようにしたが、 衝突検出後の値 T q cを式 T q c = - V ♦ K vに従って箅出しても良い。
本発明によれば、 下記に列記する利点を得ることがで きる。
( i ) デジタルシグナルプロセッサのサーボ制御動作 実行周期の各々において衝突の有無を判別するようにし たので、 衝突発生をその直後のサーボ制御周期において 検出でき、 従って、 衝突検出上の時間的遅れは、 最大で デジタルシグナルプロセッサの 1サーボ制御動作実行周 期に等し く、 各種機械に装備される制御装置の主コンビ ユータによ り衝突検出を行う場合に比べて、 衝突検出を 迅速に行える。
( i i ) 衝突発生を検出した直後およびそれ以降にお いて、 値 「 0」 の トルク指令または衝突発生後の実際モ ータ速度に比例する大きざでかつ実際モータ回転方向と は逆方向に作用する トルク指令を送出するようにしたの で、 衝突発生時にモータ回転を迅速かつ円滑に停止でき、 衝突によ る機械各部の損傷を最小限にとどめることがで きる。
( i i i ) 衝突判別基準値を指令速度の大きさ， 機械 作動部の動作の種別などに即して可変設定するようにし たので、 各種機械が如何なる運転状態で運転されている 場合にも迅速に衝突発生を検出でき、 しかも誤検出を来 すことがない。

Claims

請 求 の 範 囲
1. サーボモータの駆動力に関連する衝突判別パラメ一 タを周期的に検出する工程 （ a ) と、 前記衝突判別パ ラメータの変化高を周期的に検出する工程 （ b ) と、 前記検出した衝突判別バラメータの変化高が判別基準 値を越えたとき衝突発生を判別する工程 （ c ) と、 衝 突発生を判別したとき前記サーボモータの回耘を停止 させる工程 （ d〉 とを備えるサーボモータによ り駆動 される機械における衝突検出 ♦ 駆動停止方法。
2. 各種制御動作を周期的に実行するための主コンビュ ータを有する制御装箧を含む機械に適用されるもので、 前記工程 （ a ) ， （ b〉 を前記主コンピュータの制御 動作実行周期よ り短く かつ互いに同一の検出周期で実 行し、 前記工程 （ b ) において前回および今回の検出 周期での衝突判別パラメータの検出値の偏差を検出す る請求の範囲第 1項記載の衝突検出 * 駆動停止方法。
3. 前記衝突判別パラメータは、 前記サーボモータに送 出される指令速度と前記サーボモータの実際回転速度 との偏差と、 前記サーポモータに送出ざれる トルク指 令値とのいずれかである請求の範囲第 1 項記載の衝突 検出 ♦ 駆動停止方法。
4. 前記工程 （ d ) において、 値が 0のトルク指令を送 出する請求の範囲第 1 項記載の衝突検出 ♦ 駆動停止方 法。
5. 前記工程 （ d ) において、 大きさが実際サーボモー タ回転速度に比例しかつ実際サーボモータ回転方向と は逆方向に作用する トルク指令を送出する請求の範囲 第 1項記载の衝突検出 ♦ 駆動停止方法。
前記判別基準値は指令速度に応じて可変設定される 請求の範囲第 1 項記載の衝突検出 ♦駆動停止方法。. 前記判別基準値は動作指令の種別に応じて可変設定 される請求の範囲第 1項記載の衝突検出 ♦ 駆動停止方 法。
. 前記サーボモータを駆動制御するためのサーボ制御 系における位笸偏差が過大になったときに前記サーポ モータの励磁を解く誤差過大ァラ一ム檨能を備えた制 御装置を含む機械に適用されるものであって、 前記ェ 程 （ c ) において衝突を判別したとき前記誤差過大ァ ラーム機能を無効にする工程 （ e ) をさらに含む請求 の範囲第 1項記載の衝突検出 ♦ 駆動停止方法。
. 前記サーボモータを正常逞耘しつつ前記衝突判別パ ラメータの値を周期的に検出し、 夫々の検出周期にお い t前回検出時点から今回検出時点までの前記衝突判 別パラメータ値の変化高を算出しかつ記憶し、 前記モ ータ運転の終了後に求めた前記パラメータ変化高の最 大値を前記判別墓準値として設定する工程 （ f ) をさ らに含む請求の範囲第 1項記載の衝突検出 ♦ 駆動停止 方法。
0 . 前記工程 （ c〉 は前記衝突判別パラメータ値の変 化高が前記判別基準値を越えたか否かを周期的に判別 する工程 （ c 1 ) を含み、 前記方法は、 前記工程 （ a ) で検出した前記衝突判別パラメータ値と所定値との和 を算出する工程 （ g ) をさらに舍み、 前記工程 （ g ) で算出した前記和を次の周期での工程 （ c 1 ) での前 記判別基準値と して用いる請求の範囲第 1項記载の衝 突検出 ♦ 駆動停止方法。
PCT/JP1989/000821 1988-08-11 1989-08-11 Impingement detection/driving stop method in machine driven by servo motor WO1990001732A1 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1019900700628A KR900702425A (ko) 1988-08-11 1990-03-24 서어보 모터에 의해 구동되는 기계에 있어서의 충돌 검출·구동 정지방법

Applications Claiming Priority (4)

Application Number Priority Date Filing Date Title
JP19882388 1988-08-11
JP63/198823 1988-08-11
JP63/328866 1988-12-28
JP63328866A JPH02146602A (ja) 1988-08-11 1988-12-28 サーボモータにより駆動される被駆動体の衝突検出・停出方法

Publications (1)

Publication Number Publication Date
WO1990001732A1 true WO1990001732A1 (en) 1990-02-22

Family

ID=26511195

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP1989/000821 WO1990001732A1 (en) 1988-08-11 1989-08-11 Impingement detection/driving stop method in machine driven by servo motor

Country Status (5)

Country Link
US (1) US5214362A (ja)
EP (1) EP0380707A4 (ja)
JP (1) JPH02146602A (ja)
KR (1) KR900702425A (ja)
WO (1) WO1990001732A1 (ja)

Families Citing this family (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5304906A (en) * 1989-12-26 1994-04-19 Fanuc Ltd. Collision detecting method using an observer
JP2608161B2 (ja) * 1990-03-29 1997-05-07 ファナック株式会社 産業用ロボットの停止制御方法
IT1244315B (it) * 1990-09-12 1994-07-08 Castel Mac Spa Dispositivo elettronico di controllo della velocita' e senso di rotazione di un motore per macchina per la produzione di ghiaccio in scaglie
JP2732159B2 (ja) * 1991-10-29 1998-03-25 ファナック株式会社 異常負荷検出方法
EP0578062A3 (en) * 1992-06-29 1995-05-10 Sunny Co Ltd Control system for driving an object.
JP2871993B2 (ja) * 1993-03-31 1999-03-17 日本電気株式会社 サーボモータの位置制御装置
DE19514257C2 (de) * 1995-04-15 2003-09-18 Kostal Leopold Gmbh & Co Kg Verfahren zur Überwachung des Bewegungsvorgangs von motorisch verstellbaren Gegenständen
US5796221A (en) * 1994-07-14 1998-08-18 Paper Converting Machine Company Overload detector for servo driven machines
EP0791872A1 (de) * 1996-02-27 1997-08-27 Siemens Aktiengesellschaft Einrichtung und Verfahren zur Überwachung von Kupplungen und/oder Riemengetrieben bei Achsen oder Spindeln an Werkzeugmaschinen, Robotern oder dergleichen
US6289256B1 (en) * 1997-01-16 2001-09-11 Matsushita Electric Industrial Co., Ltd. Method and apparatus for mounting parts
JPH1133960A (ja) * 1997-07-17 1999-02-09 Fanuc Ltd ロボット制御方法
DE19756752A1 (de) * 1997-12-19 1999-06-24 Heidenhain Gmbh Dr Johannes Verfahren und Schaltungsanordnung zur Überprüfung von Motorbremsen
US6246924B1 (en) * 1998-11-30 2001-06-12 Honda Of America Mfg., Inc. Apparatus and method for automatically realigning an end effector of an automated equipment to prevent a crash
US6242879B1 (en) * 2000-03-13 2001-06-05 Berkeley Process Control, Inc. Touch calibration system for wafer transfer robot
JP2000277039A (ja) * 1999-03-26 2000-10-06 Toshiba Corp 画像表示装置およびその製造方法
DE10026991A1 (de) * 2000-05-31 2001-12-13 Bosch Gmbh Robert Verfahren zur Positionierung einer fremdkraftbetätigten Schliessfläche
DE10226853B3 (de) * 2002-06-15 2004-02-19 Kuka Roboter Gmbh Verfahren zum Begrenzen der Krafteinwirkung eines Roboterteils
US7173390B2 (en) * 2002-08-06 2007-02-06 Assembléon N.V. Method of detecting a malfunction during a displacement of an element by means of a drive system, and device suitable for carrying out such a method
JP2006154998A (ja) * 2004-11-26 2006-06-15 Fanuc Ltd 制御装置
DE102005015317B4 (de) 2005-04-01 2007-02-01 Siemens Ag Verfahren und Steuereinrichtung zur gezielten Reaktion bei einem Kontakt zwischen einem Maschinenelement einer Maschine mit einem Gegenstand
DE102007059481A1 (de) * 2007-12-11 2009-06-18 Kuka Roboter Gmbh Verfahren und Vorrichtung zur Bereichsüberwachung eines Manipulators
JP5367623B2 (ja) * 2010-03-15 2013-12-11 オムロン株式会社 サーボシステム、サーボモータ駆動装置、セーフティユニットおよびサーボシステムの制御方法
ITMI20111868A1 (it) * 2011-10-13 2013-04-14 Gima Spa Procedimento per il controllo dell'interferenza e/o collisione d'organi meccanici e sistema connesso
CN103371837B (zh) * 2012-04-27 2017-05-24 Ge医疗系统环球技术有限公司 用于往复式运动装置的抗碰撞方法和装置
EP2952300A1 (en) * 2014-06-05 2015-12-09 Aldebaran Robotics Collision detection
EP3218925B1 (en) 2014-11-10 2020-12-30 Brooks Automation, Inc. Tool auto-teach method and apparatus
JP6517567B2 (ja) * 2015-03-31 2019-05-22 日本電産サンキョー株式会社 サーボモータ制御装置及び衝突検出方法
EP3579066A1 (en) * 2018-06-06 2019-12-11 Agie Charmilles SA Collision protection method
CN109732599B (zh) * 2018-12-29 2020-11-03 深圳市越疆科技有限公司 一种机器人碰撞检测方法、装置、存储介质及机器人
JP6620380B1 (ja) * 2019-02-12 2019-12-18 株式会社安川電機 モータ制御システム
CN116392259B (zh) * 2023-04-28 2024-06-04 极限人工智能有限公司 同轨道双驱动模块的行程和防撞监测方法、系统和机器人
CN117891208A (zh) * 2024-01-10 2024-04-16 清能德创电气技术（北京）有限公司 一种伺服系统碰撞检测方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS54141971A (en) * 1978-04-27 1979-11-05 Toshiba Corp Numerical control device
JPS58136283A (ja) * 1982-02-05 1983-08-13 Matsushita Electric Ind Co Ltd 可変速電動機の保護装置
JPS60237875A (ja) * 1984-05-09 1985-11-26 Graphtec Corp サ−ボ制御装置の保護回路
JPS61203882A (ja) * 1985-03-05 1986-09-09 Toshiba Mach Co Ltd 溶融型熱転写記録装置
JPS61204707A (ja) * 1985-03-08 1986-09-10 Amada Metoretsukusu:Kk サ−ボモ−タ駆動装置の制御特性計測装置
JPH0673703A (ja) * 1992-08-24 1994-03-15 Sumitomo Rubber Ind Ltd ゴム粉砕粒状物構造体
JPH06282404A (ja) * 1993-03-25 1994-10-07 Hitachi Software Eng Co Ltd ウィンドウの制御方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS5549702A (en) * 1978-10-06 1980-04-10 Hitachi Ltd Trip reliability improving system
JPS5565110A (en) * 1978-11-13 1980-05-16 Fuji Photo Optical Co Ltd Distance meter
US4375611A (en) * 1980-06-14 1983-03-01 Burroughs Corporation Protected servomechanism
DE3122621C2 (de) * 1981-06-06 1987-04-09 Dr. Johannes Heidenhain Gmbh, 8225 Traunreut Überwachungseinrichtung
JPS5969297A (ja) * 1982-10-08 1984-04-19 株式会社日立製作所 工業用ロボツトの安全装置
US4904999A (en) * 1983-03-28 1990-02-27 The Boeing Company Fault monitoring system for aircraft power control units
US4599698A (en) * 1983-05-02 1986-07-08 United Technologies Corporation Aircraft trim actuator shutdown monitor system and method
US4582438A (en) * 1984-02-29 1986-04-15 Tokyo Juki Industrial Co., Ltd. Type wheel-type selector motor coupling detection device in printer
JPS60230204A (ja) * 1984-04-27 1985-11-15 Toyoda Mach Works Ltd 可動体移動制御装置における保安装置
JPH063994B2 (ja) * 1984-10-05 1994-01-12 株式会社日立製作所 複数台デイジタルサーボの制御方法
JPS61143803A (ja) * 1984-12-17 1986-07-01 Toshiba Corp ロボツトの制御装置
US4733149A (en) * 1985-05-31 1988-03-22 Kollmorgen Technologies Corporation Adaptive control system
US4695941A (en) * 1985-07-29 1987-09-22 General Electric Company Loss of electrical feedback detector
JPS62256007A (ja) * 1986-04-30 1987-11-07 Mitsubishi Electric Corp 故障診断装置
JP2638594B2 (ja) * 1986-11-08 1997-08-06 フアナツク株式会社 デイジタルサーボシステム
US4794309A (en) * 1987-08-26 1988-12-27 Bailey Japan Co., Ltd. Electric actuator for a control valve
JPH01116704A (ja) * 1987-10-30 1989-05-09 Fanuc Ltd 産業ロボットの動作制御方式

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS54141971A (en) * 1978-04-27 1979-11-05 Toshiba Corp Numerical control device
JPS58136283A (ja) * 1982-02-05 1983-08-13 Matsushita Electric Ind Co Ltd 可変速電動機の保護装置
JPS60237875A (ja) * 1984-05-09 1985-11-26 Graphtec Corp サ−ボ制御装置の保護回路
JPS61203882A (ja) * 1985-03-05 1986-09-09 Toshiba Mach Co Ltd 溶融型熱転写記録装置
JPS61204707A (ja) * 1985-03-08 1986-09-10 Amada Metoretsukusu:Kk サ−ボモ−タ駆動装置の制御特性計測装置
JPH0673703A (ja) * 1992-08-24 1994-03-15 Sumitomo Rubber Ind Ltd ゴム粉砕粒状物構造体
JPH06282404A (ja) * 1993-03-25 1994-10-07 Hitachi Software Eng Co Ltd ウィンドウの制御方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP0380707A4 *

Also Published As

Publication number Publication date
KR900702425A (ko) 1990-12-07
US5214362A (en) 1993-05-25
JPH02146602A (ja) 1990-06-05
EP0380707A1 (en) 1990-08-08
EP0380707A4 (en) 1992-07-08

Similar Documents

Publication Publication Date Title
WO1990001732A1 (en) Impingement detection/driving stop method in machine driven by servo motor
WO1989008538A1 (en) Method of detecting collision of moving portions driven by servo motor
JP3212571B2 (ja) 産業用ロボット
JP2732159B2 (ja) 異常負荷検出方法
KR0144650B1 (ko) 서보모터의 이상부하 검출 제어방법
WO2003068464A1 (fr) Procede de commande d'entrainement et controleur d'entrainement
JP3367641B2 (ja) ロボットの制御装置
JPWO2005035205A1 (ja) ロボットの制御装置
JPH09174482A (ja) ロボットの故障診断方法
EP1046470B1 (en) Industrial robot with means for detecting collision and preventing re-collision
JPH1170490A (ja) 産業用ロボットの衝突検出方法
JPH09282020A (ja) サーボモータ駆動装置
JPH1177580A (ja) ロボットの制御装置
WO1990008353A1 (en) Anomaly detecting method in a servo system
JPS63245389A (ja) ロボツトの制御方法
JPH08211943A (ja) 外乱オブザーバを使用したバックラッシュ補正方法
JPH01310889A (ja) 産業用ロボットの制御装置
JP3607009B2 (ja) 産業用ロボットの故障予知方法
JP3436087B2 (ja) ロボットの制御方法および装置
JPH11245191A (ja) 産業用ロボットの駆動軸制御方法及びその装置
US20220118614A1 (en) Force-limited movement of at least one element of a production machine in manual operation
JPH09106304A (ja) 工作機械の作業領域制御方法
JP3165087B2 (ja) 産業用ロボットの故障検出方法
JP2001202134A (ja) 制御装置、制御方法、および、制御装置の警報出力方法
JP2001022446A (ja) サーボ制御装置

Legal Events

Date Code Title Description
AK Designated states

Kind code of ref document: A1

Designated state(s): KR US

AL Designated countries for regional patents

Kind code of ref document: A1

Designated state(s): AT BE CH DE FR GB IT LU NL SE

WWE Wipo information: entry into national phase

Ref document number: 1989909236

Country of ref document: EP

WWP Wipo information: published in national office

Ref document number: 1989909236

Country of ref document: EP

WWW Wipo information: withdrawn in national office

Ref document number: 1989909236

Country of ref document: EP