JPS61231611A - Controller of robot - Google Patents

Abstract

PURPOSE:To attain a smooth tracking operating by an external force by providing a viscosity compensation circuit to a tracking force compensating circuit at the relax control of a robot. CONSTITUTION:A tracking force compensating circuit 60 is provided with a viscosity compensating circuit 61, a moment of inertia compensating circuit 62 and a dynamic friction compensating circuit 63. The viscosity compensation circuit 61 outputs a compensation command to generate an axial torque cancelling the viscosity resistance generate by a robot joint shaft. Then outputs of the compensating circuits 61-63 are added by an adder circuit 64 and the result is outputted as a tracking compensation command CS3. The command CS3 controls a drive current of a motor driving the joint shaft. Thus, the axial torque cancelling the viscous resistance generated in the joint shaft at the tracking operation and the robot follows an external force with almost no resistance. Thus, the smooth tracking by an external force is attained at the relaxing control.

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明はロボットの制御装置に関し、特にロボットの
可動部を外力によって自由に動かし得る状態にする、所
謂力抜き制御とも云うべき制御を行ない得るロボットの
制御装置に関する。[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a robot control device, and in particular is capable of performing so-called stress relief control, which allows the movable parts of the robot to be moved freely by external force. This invention relates to a robot control device.

〔従来の技術〕[Conventional technology]

近年、各種の産業用ロボットが工場の製造ラインで使用
されるようになり、組立（アッセンブリ）ロボットも実
用化されつつある。In recent years, various industrial robots have come to be used on factory production lines, and assembly robots are also being put into practical use.

しかし、従来から組立作業ラインに多く用いられている
コンテイニアスラインにロボットを配置して、コンベア
上を連続して流れる物品（ワーク）の組付は作業を行な
わせるには、コンベアとロボットとの間で動きの同期を
とる必要があり、ロボット自体の作業に係わる制御とコ
ンベアとの間の同期をとる制御とを同時に行なうのは非
常に難かしい。However, if a robot is to be placed on a continuous line, which has traditionally been widely used in assembly work lines, to assemble objects (workpieces) that flow continuously on a conveyor, it is necessary to It is very difficult to control the work of the robot itself and synchronize the conveyor at the same time.

そこで、ロボットがある作業を行なう間、アーム等の可
動部を外力によって自由に動かし得る「力抜き状態」に
すれば、特に同期制御を行なわなくてもコンベアーＬの
ワークの移動に追従させることができる。Therefore, if the movable parts such as arms are placed in a "relaxed state" where they can be freely moved by external force while the robot is performing a certain task, it is possible to have the robot follow the movement of the workpiece on the conveyor L without any particular synchronous control. can.

このような力抜き制御が可能なロボットの制御装置とし
ては、従来例えば特開昭５８−２０６３８９号公報に記
載されているようなものがある。As a robot control device capable of such force-relaxation control, there is a conventional one such as that described in, for example, Japanese Unexamined Patent Publication No. 58-206389.

この装置は、多軸ロボットにおける複数の可動部のうち
の予め指定した可動部を駆動するアクチュエータ、例え
ばモータへの駆動電流を遮断することによって、その可
動部を外力によって自由に動かし得るようにしたもので
ある。This device cuts off the drive current to an actuator, such as a motor, that drives a pre-designated movable part out of multiple movable parts in a multi-axis robot, so that the movable part can be moved freely by external force. It is something.

しかしながら、このような従来の力抜き制御可能なロボ
ットの制御装置にあっては、ロボットを駆動する例えば
モータへの駆動電流（パワー）を遮断するようにしてい
たため、その遮断手段として大電流用の接点容量の大き
い大型で高価なマグネットスイッチを用いなければなら
ないばかりか、それを用いることによって接点のオン・
オフ、時にモータに流れる突入電流を肋届する対策を施
したり、接点のメンテナンスを頻繁に行なう必要がある
などの問題点があった。However, in conventional control devices for robots capable of stress relief control, the drive current (power) to the motor that drives the robot, for example, is cut off, and as a means of cutting off the power, a large current Not only is it necessary to use a large and expensive magnetic switch with a large contact capacity, but using it also makes it difficult to turn on and off the contact.
Problems include the need to take measures to prevent the inrush current that flows into the motor when the motor is off, and the need to frequently perform maintenance on the contacts.

そこで、ロボットにおけるアーム等の可動部の制御は、
一般に速度指令値と可動部の速度検出系からの速度フィ
ードバック値との偏差に基づく指令値に応じて可動部を
駆動するモータの駆動電流を制御するようになっている
ので、この速度指令値と速度フィードバック値との偏差
に基づく指令値を実際の速度指令値と速度フィードバッ
ク値の値に拘らず零（ゼロ）にすることによって、可動
部を外力により自由に動かし得るようにするロボットの
制御装置を、本出願人が先に特許出願している（特願昭
５９−２６５３５３号）。Therefore, the control of movable parts such as arms in robots is
Generally, the drive current of the motor that drives the moving part is controlled according to the command value based on the deviation between the speed command value and the speed feedback value from the speed detection system of the moving part. A robot control device that allows movable parts to be moved freely by external force by setting a command value based on a deviation from a speed feedback value to zero regardless of the actual speed command value and speed feedback value. The present applicant has previously filed a patent application for this (Japanese Patent Application No. 59-265353).

このようにすれば、偏差に基づく指令値を零にするため
の切換制御手段と、して、は、接点容量が小さ、く小型
で安蝉なリレースイッチ等を使用する、こ。In this way, a small and inexpensive relay switch or the like with small contact capacity can be used as the switching control means for zeroing the command value based on the deviation.

とができ、接点の切換時にモータに突入電流が流れるこ
とがないためその防止対策を・施さなくて済み、一点の
メンテナンス回、数も少なくて済むので。Since there is no inrush current flowing into the motor when the contacts are switched, there is no need to take preventive measures, and the number of maintenance required for each point can be reduced.

前述の問題点を解決することができや。Is it possible to solve the above problems?

〔発明が解決しようとする問題点〕　　　　　。[Problem that the invention aims to solve]

しかしながら、このようなロボツ、トの制御装置におい
ても、結果としてはロボットＱ可動部の各軸を駆動する
モータ・の駆動電流を零にして力抜き状態、にするだけ
であったため、そ９状態で外力によ、る埠従動作を行な
わせると１．外力によるロボットの可動部の変位に対し
てその関節軸（駆動力、伝達部や軸受部を含む）に介在
するグリース等により粘性抵抗が生ずるため、可動部を
自由に軽く動かすことができず、スムーズな追従動作が
できなくなる場合があり、この粘性抵抗が大きいとハン
ドに取付けた工具がワークから外れてしまうこともある
という問題点があった。However, even in such a control device for a robot, the result is that the drive current of the motor that drives each axis of the movable part of the robot Q is reduced to zero, and the force is relaxed. When the following motion is performed by an external force, 1. When a robot's movable parts are displaced by an external force, viscous resistance is generated by the grease, etc. present in the joint axes (including driving force, transmission parts, and bearing parts), making it impossible to move the movable parts freely and lightly. There have been problems in that smooth follow-up motion may not be possible, and if this viscous resistance is large, the tool attached to the hand may come off the workpiece.

この発明は、この問題点を解決して、ロボットの力抜き
制御時に外力によるスムーズな追従動作を可能にするこ
とを目的とする。It is an object of the present invention to solve this problem and enable a smooth follow-up operation using an external force during force relief control of a robot.

〔問題点を解決するための手段〕[Means for solving problems]

そのため、この発明によるロボットの制御装置は、速度
指令値と速度フィードバック値との偏差に基づく指令値
を実際値に拘らず零にすることによって可動部の力、抜
き制御を・行なうと共に、該可動部の変位時にその可動
部の関節軸に生ずる粘性抵抗に対応するトルクを発生さ
せるための粘性補償指令値を出力す、る粘性補償回路を
設け、少くとも上記力抜き制御時に、その粘性補償指令
値によって上記関節、軸を駆動するモータの、駆動電流
を制御して、ロボットの可動部を外力により自由に軽く
動かし得るようにしたものである。Therefore, the robot control device according to the present invention controls the force and pull-out of the movable part by setting the command value based on the deviation between the speed command value and the speed feedback value to zero regardless of the actual value, and also controls the force of the movable part. A viscosity compensation circuit is provided that outputs a viscosity compensation command value to generate a torque corresponding to the viscous resistance generated in the joint axis of the movable part when the part is displaced, and the viscosity compensation command is output at least during the above-mentioned force release control. By controlling the drive current of the motor that drives the joints and axes according to the value, the movable parts of the robot can be freely and lightly moved by external force.

〔実　施　例Ｊ 以下、この発明の実施例を添付図面を・参照しながら説
明する。[Embodiment J] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings.

先ず、第２図を参照してこの実施例に使用するロボット
の構成及びその作業について説明する。First, the configuration of the robot used in this embodiment and its work will be explained with reference to FIG.

図中、１は垂直多関節型ロボットであり、図示しない台
座上に固定された基部２に垂直に立設した原軸３と、こ
の原軸に対して直角に固定された肘軸を兼ねたモータ４
に連結された第１アーム（上腕）５と、この第１アーム
の先端部に対軸６によって回動自在に連結された第２ア
ーム（下腕）７と、この第２アーム７の先端部に手首軸
８によって回動自在に連結されたハンドＳ等から成る。In the figure, 1 is a vertically articulated robot, which has a base 2 fixed on a pedestal (not shown), and a base 3 that stands perpendicularly to it, and an elbow shaft that is fixed at right angles to this base. motor 4
A first arm (upper arm) 5 connected to the first arm, a second arm (lower arm) 7 rotatably connected to the tip of the first arm by a shaft 6, and a tip of the second arm 7. The hand S is rotatably connected to the wrist shaft 8 by a wrist shaft 8.

これを模式的に示すと第３図のようになる。This is schematically shown in FIG. 3.

原軸３は、モータ１０によって水平面内で矢示Ａ方向に
回転される水平関節軸である。The original axis 3 is a horizontal joint axis that is rotated by the motor 10 in the direction of arrow A within a horizontal plane.

第１アーム５は、モータ４によって矢示Ｂ方向に、第２
アーム７はモータ１１によって矢示Ｃ方向に、ハンド９
は第２アーム７に内蔵された図示しないモータによって
矢示り方向にそれぞれ垂直面内で回動し、これらを連結
する肘軸と対軸６及び手首軸８が垂直関節軸である。The first arm 5 is moved in the direction of arrow B by the motor 4 to the second arm 5.
The arm 7 is moved by the motor 11 in the direction of arrow C, and the hand 9
is rotated in a vertical plane in the direction of the arrow by a motor (not shown) built into the second arm 7, and the elbow shaft, the opposite shaft 6, and the wrist shaft 8 that connect these are vertical joint axes.

なお、モータ４，１０．１１及びハンドＳを回動させる
図示しない手首軸駆動モータは、いずれもＤＣサーボモ
ータを使用する。そして、これらの各モータの駆動力を
伝達する減速機としては、逆伝達効率の比較的高いもの
（例えばベベルギヤ機構によるもの）を使用する。Note that the motors 4, 10, 11 and a wrist shaft drive motor (not shown) that rotates the hand S use DC servo motors. As a speed reducer for transmitting the driving force of each of these motors, a speed reducer with relatively high reverse transmission efficiency (for example, one using a bevel gear mechanism) is used.

また、これらの各モータの出力軸には、その回転速度を
検出するためのタコジェネレータ及び回転角度を検出す
るためのポテンショメータがそれぞれ取付けられている
。Further, a tacho generator for detecting the rotation speed and a potentiometer for detecting the rotation angle are attached to the output shaft of each of these motors.

ハンド９は、手首軸８に連結されたホルダ１２に固定し
たエアシリンダ１３の回り止めを施こしたピストンロッ
ドに、先端にボルトを衝えて締め付けるソケット１４ａ
を備えたナツトランナ１４を取付けて構成し、さらに、
ホルダ１２に一対のリミットスイッチ１５．１６を取付
けて、ナツトランナ１４に付設したドッグ１４ｂによっ
て作動されるようにして、ナツトランナ１４の上昇限位
置と下降限位置を検出できるようにしている。The hand 9 has a socket 14a for tightening a bolt at the tip of the piston rod, which is fixed to a holder 12 connected to the wrist shaft 8 and which prevents rotation of the air cylinder 13.
A nut runner 14 is installed and configured, and further,
A pair of limit switches 15 and 16 are attached to the holder 12 and are actuated by a dog 14b attached to the nut runner 14, so that the upper limit position and lower limit position of the nut runner 14 can be detected.

一方、１７はコンティニュアスコンベア（以下単に「コ
ンベアＪと云う）であり、所要位置に所定の姿勢で位置
決め固定した例えばエンジンブロック等のワーク１８を
載せて、ロボット１のワークエリア内を矢示Ｅ方向に所
定の速度で搬送するようになっている。On the other hand, 17 is a continuous conveyor (hereinafter simply referred to as "conveyor J"), on which a workpiece 18 such as an engine block, which is positioned and fixed in a predetermined position and in a predetermined posture, is placed, and the workpiece 18 is moved within the work area of the robot 1 by an arrow. It is designed to be transported at a predetermined speed in the E direction.

そして、このコンベア１７によって搬送されるワーク１
日には、作業対象であるボルト１日（例えばシリンダヘ
ッドにヘッドカバーを固定するためのボルト）をセット
してあり、このボルト１日を締め付ける作業を、ハンド
９にナツトランナ１４を取付けたロボット１に行なわせ
ようとするものである。The workpiece 1 transported by this conveyor 17
A bolt to be worked on (for example, a bolt for fixing a head cover to a cylinder head) is set in the robot 1, and the robot 1, which has a nut runner 14 attached to the hand 9, performs the task of tightening the bolt. It's something I'm trying to get done.

さらに、２０はフロアに立設したステー２１の上端部に
固定したボルト通過検知器であり、ロボット１が予め定
めた図示のような位置で待機している時に、コンベア１
７によって搬送されるワーク１８上のボルト１日が通過
した時にそれを検知する。Furthermore, 20 is a bolt passage detector fixed to the upper end of a stay 21 installed on the floor, and when the robot 1 is waiting at a predetermined position as shown in the figure, the conveyor 1
It is detected when the bolt on the workpiece 18 conveyed by the bolt 7 has passed.

なお、このボルト通過検知器２０としては、例えば反射
型の光電スイッチなどを用いる。Note that as this bolt passage detector 20, for example, a reflective photoelectric switch or the like is used.

また、このボルト通過検知器２０のボルト１日を検知す
る位置とナツトランナ１４との位置関係は、ボルト通過
検知器２０がボルト１日の通過を検知した時点で、ナツ
トランナ１４を下降限まで下降させた時に頂度ソケット
１４．がボルト１Ｓを衝え込むような関係にする。Further, the positional relationship between the position of the bolt passage detector 20 that detects the bolt 1 day and the nut runner 14 is such that the nut runner 14 is lowered to the lower limit when the bolt passage detector 20 detects the passage of the bolt 1 day. When the apex socket 14. Make the relationship such that Bolt 1S is pushed into it.

ボルト通過検知器２０の代りに、ボルト１９に対応する
コンベア１７の所要位置にドッグを設けておき、そのド
ッグによってコンベア１７に沿う所要位置の固定部に取
り付けたリミットスイッチを叩くようにして、このリミ
ットスイッチのオンによってボルト１９の通過を検知す
るようにしても良い。Instead of the bolt passage detector 20, a dog is provided at a predetermined position on the conveyor 17 corresponding to the bolt 19, and the dog hits a limit switch attached to a fixed part at a predetermined position along the conveyor 17. Passage of the bolt 19 may be detected by turning on the limit switch.

次に、第１図を参照して第２図に示したロボット１の制
御装置の実施例を説明する。Next, an embodiment of the control device for the robot 1 shown in FIG. 2 will be described with reference to FIG.

第１図において、２３はマイクロコンピュータ等を用い
た中央処理部であり、ロボット１の全般的な制御を司っ
ている。In FIG. 1, numeral 23 is a central processing unit using a microcomputer or the like, and is in charge of overall control of the robot 1.

すなわち、位置指令レジスタ２４９位置制御部２５、速
度制御部２６．及び電流制御部２７等によって構成され
た第１アーム５を回動させる肘軸を駆動するモータ４用
のサーボ制御部と５このサーボ制御部と全く同様に、位
置指令レジスタ３４゜位置制御部３５．速度制御部３６
．及び電流制御部３７等によって構成された原軸３を駆
動するモータ１０用のサーボ制御部と、図示を省略した
が、やはりこれらのサーボ制御部と同様に構成された第
２アーム７を回動させる対軸駆動モータ１１用のサーボ
制御部及び手首軸駆動モータ用のサーボ制御部をそれぞ
れ制御すると共に、第２ｖ４に示したハンド９に取付け
たエアシリンダ１３及びナツトランナ１４の制御、すな
わちエアシリンダ１３によるナツトランナ１４の昇降制
御及びその内蔵モータによるソケット１４．の回転・停
止の制御も行なう。That is, the position command register 249, the position control section 25, the speed control section 26. A servo control unit for the motor 4 that drives the elbow shaft that rotates the first arm 5, which is composed of a current control unit 27 and the like; ．． Speed control section 36
．． and a servo control unit for the motor 10 that drives the original shaft 3, which is configured by a current control unit 37, etc., and a second arm 7 that rotates the second arm 7, which is also configured in the same manner as these servo control units, although not shown. In addition to controlling the servo control unit for the twin shaft drive motor 11 and the servo control unit for the wrist shaft drive motor, respectively, the control unit controls the air cylinder 13 and nut runner 14 attached to the hand 9 shown in 2v4, that is, the air cylinder 13 Elevation control of the nut runner 14 and socket 14 by its built-in motor. It also controls the rotation and stopping of the

次に、モータ４用のサーボ制御部において、位置レジス
タ２４には、中央処理部２３からの第１アーム５の目標
位置指令値が逐次更新されながら書き込まれる。Next, in the servo control section for the motor 4, the target position command value of the first arm 5 from the central processing section 23 is written into the position register 24 while being updated one after another.

位置制御部２５は、位置指令レジスタ２４に書き込まれ
ている第１７−４５の目標位置指令値と、モータ４の出
力軸に取付けられているポテンショメータ３０からの位
置フィードバック信号（電圧）をＡ／Ｄ変換器３１によ
ってデジタル値に変換した値、すなわち第１７−ム５の
現在位置値（第３図の角度θに相当する）との偏差に基
づく速度指令値Ｓａを出力すると共に、目標位置指令値
と現在位置値とが一致して位置決めが完了する毎にそれ
を中央処理部２３に知らせ、中央処理部２３はそれによ
って次に目標位置指令値を出力するタイミングを測って
いる。The position control unit 25 converts the 17th to 45th target position command values written in the position command register 24 and the position feedback signal (voltage) from the potentiometer 30 attached to the output shaft of the motor 4 into an A/D converter. It outputs a speed command value Sa based on the deviation from the value converted into a digital value by the converter 31, that is, the current position value of the 17-m 5 (corresponding to the angle θ in FIG. 3), and also outputs the target position command value. Each time the current position value matches the current position value and positioning is completed, this is notified to the central processing unit 23, and the central processing unit 23 uses this to measure the timing for outputting the next target position command value.

速度制御部２６は、後述する指令値制御回路４５を介し
て入力される位置制御部２５からの速度指令値Ｓａと、
モータ４の出力軸に取付けられたタコジェネレータ２日
からの速度フィートノ１ツク値との偏差に基づく電流指
令値Ｓ１を出力する。The speed control unit 26 receives a speed command value Sa from the position control unit 25 that is input via a command value control circuit 45, which will be described later.
A current command value S1 is output based on the deviation from the speed foot check value from the tachogenerator attached to the output shaft of the motor 4.

電流制御部２７は、速度制御部２６からの電流指令値Ｓ
１を加算回路！Ｉ２を介して入力し、モータ４に流れる
駆動電流を検出する電流検出器２８からの電流フィード
バック値との偏差に基づく駆動電流をモータ４に流す。The current control unit 27 receives the current command value S from the speed control unit 26.
Add 1 circuit! A drive current is applied to the motor 4 based on the deviation from the current feedback value inputted via I2 and from the current detector 28 that detects the drive current flowing to the motor 4.

したがって、位置指令レジスタ２４１位置制御部２５．
速度制御部２６．及び電流制御部２７等からなるモータ
４用のサーボ制御部は、後述する指令値制御回路４５が
位置制御部２５からの速度指令値とタコジェネレータ２
日からの速度フィードバック値とをそのまま速度制御部
２６に出力している限りにおいては、中央処理部２３か
らの目標位置指令値に基づいてモータ４を駆動して、第
１アーム５をプレイバック制御（位置決め制御）するこ
とができる。Therefore, position command register 241 position control section 25.
Speed control section 26. In the servo control unit for the motor 4, which includes a current control unit 27, etc., a command value control circuit 45, which will be described later, uses a speed command value from the position control unit 25 and a tacho generator 2.
As long as the speed feedback value from the previous day is output as is to the speed control unit 26, the motor 4 is driven based on the target position command value from the central processing unit 23, and the first arm 5 is controlled for playback. (positioning control).

原軸３を回転駆動するモータ１０用のサーボ制御部を構
成する各部３４〜４２も、上述したモータ４用のサーボ
制御部を構成する各部２４〜３２と全く同様に機能し、
やはり後述する指令値制御回路４５が位置制御部３５か
ら出力される速度指令値ｓｂとモータ１０の出力軸に取
付けられたタコジェネレータ３日から出力される速度フ
ィードバック値とをそのまま速度制御部３６に出力して
いる限りにおいては、中央処理部２３からの目標位置指
令値に基づいてモータ１．０を駆動して原軸３及びそれ
と一体のモータ４の向きをプレイバック制御（位置決め
制御）することができる。Each section 34 to 42 that constitutes a servo control section for the motor 10 that rotationally drives the original shaft 3 functions in exactly the same way as each section 24 to 32 that constitutes the servo control section for the motor 4 described above.
A command value control circuit 45, which will also be described later, directly transmits the speed command value sb output from the position control section 35 and the speed feedback value output from the tacho generator attached to the output shaft of the motor 10 to the speed control section 36. As long as it is outputting, the motor 1.0 is driven based on the target position command value from the central processing unit 23 to perform playback control (positioning control) of the direction of the original shaft 3 and the motor 4 integrated therewith. I can do it.

さらに、第２アーム７を駆動するモータ１１用及び手首
軸駆動モータ用の各サーボ制御部も、同様に機能□し□
て夫々各モータを駆動し、第２アーム７及びハンド９を
′プレイバック制御することができる。Furthermore, each servo control unit for the motor 11 that drives the second arm 7 and the wrist shaft drive motor also function in the same way.
The second arm 7 and the hand 9 can be controlled for playback by driving each motor respectively.

第２図のエアシリンダ１３を制御させるエアシリンダ用
の駆動回路は、公知のシリンダ操作回路からなり、□そ
の電磁・式方向切換弁を中央処理部２３からの指令で切
換え、エアの供給方向を切換えて昇降させる。ソケット
１４．の回転・停止は、中央処理部２３からの指令によ
りナツトランナ１４に内蔵するモータへの給電を制御す
ることにより行なう。The air cylinder drive circuit that controls the air cylinder 13 shown in FIG. Switch to raise and lower. Socket 14. The rotation and stopping of the nut runner 14 is performed by controlling the power supply to the motor built in the nut runner 14 based on commands from the central processing section 23.

指令値制御回路４５は、リレーコイルＬの励磁・非励磁
によって切換わる１１個の切換スイッチＳＷ１〜Ｓ・Ｗ
＋＋（モータ１１用及び手首軸駆動モータ用サーボ制御
部に介挿する４個の切換スイッチ３Ｗ５〜ＳＷ８は図示
していない）を有する電磁リレーからなる。The command value control circuit 45 includes 11 changeover switches SW1 to SW, which are switched by energizing and de-energizing the relay coil L.
It consists of an electromagnetic relay having ++ (four changeover switches 3W5 to SW8 inserted in the servo control section for the motor 11 and the wrist shaft drive motor are not shown).

この指令値制御回路４５の切換スイッチＳＷｔ。This command value control circuit 45 has a changeover switch SWt.

ＳＷ２の可動接片Ｃは夫々速度制御部２６の入力側に接
続され、各固定接点ａは夫々アースに、各固定接点すは
位置制御部２５の出力側及びタコジェネレータ２日に夫
々接続されている。The movable contacts C of SW2 are connected to the input side of the speed control section 26, each fixed contact a is connected to the ground, and each fixed contact A is connected to the output side of the position control section 25 and the tachogenerator 2, respectively. There is.

また、切換スイッチＳ　Ｗ　ａ　＝　Ｓ　Ｗ４の可動接
片Ｃは夫々速度制御部３日の入力側に接続され、各固定
接点ａはアースに、各固定接点すは位置検出部３５の出
力側及びタコジェネレータ３日に夫々接続されている。In addition, the movable contacts C of the changeover switch S W a = S W4 are connected to the input side of the speed control section 35, respectively, each fixed contact a is connected to the ground, and each fixed contact is connected to the output side of the position detection section 35 and Each is connected to a tacho generator on the third day.

図示しない切換スイッチＳ　Ｗ　ｓ　、　Ｓ　Ｗ　６及
びＳ　Ｗ７　、　Ｓ　Ｗａも、夫々モータ１１用のサー
ボ制御部及び手首軸駆動モータ用サーボ制御部において
、全く同様に接続されている。Changeover switches SW s , SW 6 and SW 7 , S Wa (not shown) are also connected in exactly the same way in the servo control unit for the motor 11 and the servo control unit for the wrist shaft drive motor, respectively.

切換スイッチＳ　Ｗｓ　、　Ｓ　Ｗ＋ｏは、後述する重
力バランス補償回路５０から出力される重力補償指令値
ｃｓ、、ｃｓ２をそれぞれ加算回路３２及びモータ１１
用の図示しないサーボ制御部における同様な加算回路に
入力するラインに介挿されて、開閉スイッチとして用い
られている。The changeover switches S Ws and S W+o output gravity compensation command values cs, cs2 output from a gravity balance compensation circuit 50, which will be described later, to the addition circuit 32 and the motor 11, respectively.
It is inserted into a line input to a similar adder circuit in a servo control unit (not shown) for use as an open/close switch.

さらに、切換スイッチＳＷｎは、後述する追従力補償回
路６０から出力さ九る追従力補償指令値Ｃ８３を加算回
路４２に入力するラインに介挿されて、開閉スイッチと
して用いられている。Further, the changeover switch SWn is inserted into a line for inputting a following force compensation command value C83 outputted from a following force compensation circuit 60, which will be described later, to the adding circuit 42, and is used as an opening/closing switch.

なお、リレーコイルＬの両端に接続したダイオードＤは
フライホイールダイオードである。Note that the diode D connected to both ends of the relay coil L is a flywheel diode.

この指令値制御回路４５は、リレーコイルＬの非励磁時
には、各切換スイッチＳＷ　１〜Ｓ　Ｗ　＋　＋の可動
接片Ｃが固定接点す側に夫々切換わっていて、実際の速
度指令値と速度フィードバック値をそのまま通過させて
、各モータ用のサーボ制御部を本来どおり位置決め動作
させるが、リレーコイルＬが励磁されると、各切換スイ
ッチＳＶ／＋　−８Ｗ＋＋の可動接片Ｃが図示のように
固定接点ａ側に切換わり、各モータ用のサーボ制御部に
おける速度制御部２Ｅｉ、３Ｇ、・・・に入力する速度
指令値と速度フィードバック値をいずれも零（アース値
）にして、その偏差に基づく電流指令値５１ｙＳ２ｔ・
・・を実際の速度指令値と速度フィードバック値の偏差
にかかわらず、換言するとモータ４，１０等の動きに関
係なくゼロにする。In this command value control circuit 45, when the relay coil L is not energized, the movable contacts C of the changeover switches SW1 to SW+ are respectively switched to the fixed contact side, and the actual speed command value and speed are The feedback value is passed through as is, and the servo control unit for each motor is operated for positioning as usual. However, when the relay coil L is energized, the movable contact C of each changeover switch SV/+ -8W++ moves as shown in the figure. The fixed contact a side is switched, and the speed command value and speed feedback value input to the speed control section 2Ei, 3G, ... in the servo control section for each motor are set to zero (earth value), and the deviation is Based on the current command value 51yS2t・
... is set to zero regardless of the deviation between the actual speed command value and the speed feedback value, in other words, regardless of the movement of the motors 4, 10, etc.

１ｂ− このように、電流指令値を実際の速度指令値と速度フィ
ードバック値にがかわらずゼロにすると、位置及び速度
フィードバック制御が効がなくなるため、各モータ４，
１０．１１等はフリーの状態になり、それによって第２
図のロボット１の原軸３、第１アーム５．第２７−ム７
、及びハンド９は外力によって自由に動かせるようにな
る。1b- In this way, if the current command value is set to zero regardless of the actual speed command value and speed feedback value, position and speed feedback control will become ineffective, so each motor 4,
10.11 mag. becomes free, thereby making the second
The main axis 3 and the first arm 5 of the robot 1 shown in the figure. 27th-m7
, and hand 9 can be moved freely by external force.

しかしながら、この場合各モータの駆動電流を完全にゼ
ロにしてしまうと、第１．第２アーム５゜７及びハンド
９の自重により、各垂直関節軸が回動され、これらの可
動部の姿勢が崩れてしまって作業ができなくなるため、
重力バランス補償回路５０を設けており、それによって
重力バランスを保って姿勢が崩れないようにしているが
、その詳細は後述する。However, in this case, if the drive current of each motor is completely reduced to zero, the first. The weight of the second arm 5°7 and the hand 9 rotates each vertical joint axis, causing the postures of these movable parts to collapse and making it impossible to work.
A gravity balance compensation circuit 50 is provided to maintain the gravity balance and prevent the posture from collapsing, the details of which will be described later.

また、水平関節軸である原軸３を駆動するモータ１０の
サーボ制御部に対して、この発明の特徴である追従動作
時における粘性補償、さらには慣性補償及び動摩擦補償
をも行なうための追従力補償回路６０を設けているが、
その詳細も後述する。Furthermore, a following force is applied to the servo control unit of the motor 10 that drives the original shaft 3, which is the horizontal joint axis, to perform viscosity compensation during the following operation, which is a feature of the present invention, as well as inertia compensation and dynamic friction compensation. Although a compensation circuit 60 is provided,
The details will be described later.

なお、指令値制御回路４５の各切換スイッチＳＷｒ〜Ｓ
Ｗ＋＋の可動接片Ｃが固定接点す側に切り換わっている
時でも、位置決め完了時には速度指令値と速度フィード
バック値の偏差が零になるが、この時には位置及び速度
フィードバック制御が効いているため、腰軸３．第１．
第２アーム５゜７等に外力が加わってモータ１０，４．
１１等が停止位置から回転されると、直ちにそれを元に
戻そうとする回転力が発生する。In addition, each changeover switch SWr to S of the command value control circuit 45
Even when the movable contact piece C of W++ is switched to the fixed contact side, the deviation between the speed command value and the speed feedback value becomes zero when positioning is completed, but at this time the position and speed feedback control is effective, so Waist axis 3. 1st.
When an external force is applied to the second arm 5.7, etc., the motors 10, 4.
11 etc. is rotated from the stopped position, a rotational force is immediately generated to return it to its original position.

また、この指令値制御回路４５においては、電流指令値
”１ｔＳ２ｔ・・・を零にするように機能している時（
この実施例ではリレーコイルＬが励磁されている時）を
以て作動状態という。In addition, in this command value control circuit 45, when the current command value "1tS2t..." is functioning to be zero (
In this embodiment, the state when the relay coil L is excited is referred to as the operating state.

４６は切換回路であり、電源Ｖｃｃの給電回路に介挿し
た切換スイッチ４７．４８とロボット１のハンド９に取
付けた上昇限りミツトスイッチ１５によって構成されて
いる。なお、リミットスイッチ１５は常閘型で、ナツト
ランナ１４が上昇限に達してドッ″グ１４ｂによって叩
かれた時にオフにな□る。Reference numeral 46 denotes a changeover circuit, which is composed of changeover switches 47 and 48 inserted into the power supply circuit of the power source Vcc, and an ascent limit switch 15 attached to the hand 9 of the robot 1. The limit switch 15 is of a constant lock type and is turned off when the nut runner 14 reaches its upper limit and is struck by the dog 14b.

この切換回路４６は、図示のように切換スイッチ４８の
可動接片ｉを固定接点ｇ側に切換えておくと、リミット
スイッチ１５がオフの時（ナツトランナ１４が上昇位置
にある時）に指令値制御回路４５のリレーコイルＬを非
励磁にし、リミットスイッチ１５がオンの時はリレーコ
イルＬを励磁する。This switching circuit 46 controls the command value when the limit switch 15 is off (when the nut runner 14 is in the raised position) by switching the movable contact i of the changeover switch 48 to the fixed contact g side as shown in the figure. The relay coil L of the circuit 45 is de-energized, and when the limit switch 15 is on, the relay coil L is energized.

また、切換スイッチ４８の可動接片ｉを固定接点り側に
切換えておくと、リミットスイッチ１５のオン・オフに
かかわらず、切換スイッチ４７によって指令値制御回路
４５の作動・不作動を切換えることができる。Furthermore, if the movable contact i of the changeover switch 48 is switched to the fixed contact side, the command value control circuit 45 can be activated or deactivated by the changeover switch 47 regardless of whether the limit switch 15 is on or off. can.

なお、この実施例においては、リレーコイルＬの励磁と
指令値制御回路４５の作動とが対応しているが、切換ス
イッチＳＷ　１〜５Ｗ１１の固定接点ａとｂの接続を逆
にすれば、リレーコイルの非励磁と指令値制御回路４５
の動作とを対応させるようにすることも可能である。In this embodiment, the excitation of the relay coil L corresponds to the operation of the command value control circuit 45, but if the connections of the fixed contacts a and b of the changeover switches SW 1 to 5W11 are reversed, the relay Coil de-energization and command value control circuit 45
It is also possible to make the operation correspond to the operation of .

次に、重力バランス補償回路５０の詳細例を第４図によ
って説明する。Next, a detailed example of the gravity balance compensation circuit 50 will be explained with reference to FIG.

この重力バランス補償回路５０は、ＣＰＵ　（中央処理
装置）５１と、プログラムメモリとしてのＲＯＭ及びデ
ータメモリとしてのＲＡＭを含むメモリ５２と、１対ず
つのＡ／Ｄ変換器５３，５４及びＤ／Ａ変換器ｓｓ、ｓ
ｅとからなるマイクロコンピュータによって構成されて
いる。The gravity balance compensation circuit 50 includes a CPU (central processing unit) 51, a memory 52 including a ROM as a program memory and a RAM as a data memory, and a pair of A/D converters 53, 54 and a D/A converter. converter ss,s
It is composed of a microcomputer consisting of e.

そして、この重力バランス補償回路５０は、モータ４の
出力軸に取付けられたポテンショメータ３０から出力さ
れる第１アーム５の水平位置からの回動角度θ１　（第
３図参照）に相当する電圧信号をＡ／Ｄ変換器５３によ
ってデジタル値に変換してＣＰＵ５１に読込み、同様に
モータ１１の出力軸に取付けられたポテンショメータ４
日から出力される第１アーム５に対する第２アーム７の
回動角度θ２　（第３図参照）に相当する電圧信号をＡ
／Ｄ変換器５４によってデジタル値に変換してＣＰＵ５
１に読込む。The gravity balance compensation circuit 50 receives a voltage signal corresponding to the rotation angle θ1 (see FIG. 3) of the first arm 5 from the horizontal position, which is output from the potentiometer 30 attached to the output shaft of the motor 4. The A/D converter 53 converts it into a digital value and reads it into the CPU 51, and the potentiometer 4 similarly attached to the output shaft of the motor 11
The voltage signal corresponding to the rotation angle θ2 (see Fig. 3) of the second arm 7 relative to the first arm 5 output from the
It is converted into a digital value by the /D converter 54 and sent to the CPU 5.
Read into 1.

そして、メモリ５２に予めロボット１の第１アーム５の
肩軸４ａから重心までの長さｊ？、及び全長！２．第２
アーム７の対軸６から重心までの長さｉ３及び全長’４
ｅ第１アーム５．第２アーム７、及びハンド９の各重量
Ｗ１＊　Ｗ２　ｉ　Ｗａ、（第３図参照）、及びｓｉｎ
θｔ、　ｃｏｓθのテーブルを記憶させておき、垂直関
節軸である肩軸４ａ及び対軸６、、における重力のモー
メント￥（Ａ）、Ｍ（Ｂ）をＣｐｕ５１が下記の演算を
行なって算出し、そ、れに抗し得る軸トルクを発生させ
るための補償指令値ＣＳ　１　ｗ　ＣＳ　２をＤ／Ａ変
換器５５．５６を介してアナログ信号に変換して出力す
る。Then, the length j? from the shoulder axis 4a of the first arm 5 of the robot 1 to the center of gravity is stored in the memory 52 in advance. , and full length! 2. Second
Length i3 from opposite axis 6 of arm 7 to center of gravity and total length '4
e first arm 5. Each weight of the second arm 7 and hand 9 W1*W2 i Wa, (see Fig. 3), and sin
A table of θt and cosθ is stored, and the CPU 51 calculates the moments of gravity ¥(A) and M(B) on the shoulder axis 4a and the opposite axis 6, which are the vertical joint axes, by performing the following calculation, Compensation command values CS 1 w CS 2 for generating shaft torque capable of resisting this are converted into analog signals via D/A converters 55 and 56 and output.

Ｍ　（Ａ）　＝　Ｋ　１　ｃｏｓθｔ　＋　Ｋ　２　ｓ
ｉｎ、（θ鳳＋０２−９０’　）Ｍ（Ｂ）＝　Ｋ２　ｓ
ｉｎ　（θ１＋０２−９０’　）但し、Ｋ、＝Ｉ　ｔ　
Ｗｔ　＋！２　（Ｗ２　＋Ｗａ　）Ｋ２　＝Ｉ　３　Ｗ
２　＋Ｉ４　Ｗａ で与えられる。このＫ　Ｉ　ｅ　Ｋ　２は定数であるか
ら、これを予めメモリ５２に格納しておくとよい。M (A) = K 1 cosθt + K 2 s
in, (θ+02-90')M(B)=K2s
in (θ1+02-90') However, K, = I t
Wt +! 2 (W2 +Wa)K2 =I 3 W
It is given by 2 +I4 Wa. Since this K I e K 2 is a constant, it is preferable to store it in the memory 52 in advance.

、このようにして、重力バランス補償回路５０から出力
される補償指令値Ｃ８１，Ｃ８，２を、力埜牟制御時に
指令値制御回路４５の切換スイッチＳ、Ｗ、１１ｔ、　
Ｓ　Ｗ　＋ｏを介して加算回路３２及びモータ１１用の
図示しないサーボ制御部における同様な加、算回路に入
力させ、速度制御部２６．・・・から出力される電流指
令値Ｓｉｔ・・・がゼロであっても、電流制御部２７．
・・・によって重力補償用の駆動電流をモータ４及び１
１に流し、第１．第２アーム５．７及リバンド９の自重
に抗し得る軸トルクを発生させる・ので、第１．第２ア
ニム５，７が重力バランスを保って姿勢を保持すること
ができる。, In this way, the compensation command values C81, C8, 2 output from the gravity balance compensation circuit 50 are changed to the changeover switches S, W, 11t, of the command value control circuit 45 during force control.
S W +o is input to an addition circuit 32 and a similar addition circuit in a servo control section (not shown) for the motor 11, and the speed control section 26. Even if the current command value Sit . . . output from the current control unit 27 .
... to control the drive current for gravity compensation to motors 4 and 1.
1. To generate an axial torque capable of resisting the weight of the second arm 5.7 and ribbon 9, the first. The second animations 5 and 7 can maintain their postures while maintaining the gravitational balance.

なお、この、実施例では、ハンド９のエアシリンダ１３
及びナツトランナ１４は、力抜き状態では、常に垂直、
（鉛直）に保持され、手首軸８に加わるハンド９の自、
重は常に鉛直方向に作用するためモーラ４ントは発生し
・ないので、手首軸単独での重力補償は行なっていない
。In addition, in this embodiment, the air cylinder 13 of the hand 9
And the nut runner 14 is always vertical in the relaxed state,
The hand 9 is held vertically and is applied to the wrist shaft 8,
Since weight always acts in the vertical direction, no mora force is generated, so gravity compensation is not performed on the wrist axis alone.

また、この重力バランス補償回路５０の機能を第１図の
中央処理部２３に持たせて、共通のＣＰＵによって時分
割処理させる゛ようにしてもよい。Further, the function of the gravity balance compensation circuit 50 may be provided in the central processing unit 23 of FIG. 1, and time-division processing may be performed by a common CPU.

次に、第１図における追従力補償回路６０の具体例を第
５図及び第６図によって説明する。Next, a specific example of the tracking force compensation circuit 60 shown in FIG. 1 will be explained with reference to FIGS. 5 and 6.

第５図に示すこの実施例における追従力補償回路６０は
、粘性補償回路６１．慣性補償回路６２゜及び動摩擦補
償回路６３と、これらの各補償回路から出力される補償
指令値のレベルを調整するたメ（７）ボ’Ｊ−Ｌ　　Ａ
ＶＲ＋　、ＶＲ２’、ＶＨ２と、その各出力を加算する
加算回路６４とによって構成されている。The following force compensation circuit 60 in this embodiment shown in FIG. 5 includes a viscosity compensation circuit 61 . (7) Bo' J-L A for adjusting the level of the inertia compensation circuit 62°, the dynamic friction compensation circuit 63, and the compensation command value output from each of these compensation circuits.
It is composed of VR+, VR2', VH2, and an adder circuit 64 that adds their respective outputs.

粘性補償回路６１は、ロボット１の可動部が変位する時
にその原軸３に発生する粘性抵抗を相殺する軸トルクを
モータ１０に発生させるための補償指令値を出力する回
路で、オペアンプＯＰ、と入力抵抗Ｒｌ　’　ｖ帰還抵
抗Ｒ２からなる増幅度が１より小さい増幅器である。The viscosity compensation circuit 61 is a circuit that outputs a compensation command value for causing the motor 10 to generate a shaft torque that offsets the viscous resistance generated in the original shaft 3 when the movable part of the robot 1 is displaced, and includes an operational amplifier OP. This is an amplifier with an amplification factor of less than 1, consisting of an input resistor Rl' and a feedback resistor R2.

そして、ロボット１の追従動作期間Ｔの間にタコジェネ
レータ３Ｓによって、原軸３の回動変位の速度に応じて
第６図（イ）に示すように発生される速度フィードバッ
ク値である電圧信号ｖ１．ｇを入力して、粘性補償指令
値として同図（ロ）に示すような入力に比例した電圧信
号Ｖａを出力する。A voltage signal v1, which is a speed feedback value, is generated by the tacho generator 3S during the follow-up operation period T of the robot 1, as shown in FIG. ．． g is input, and a voltage signal Va proportional to the input as shown in the figure (b) is output as a viscosity compensation command value.

慣性補償回路６２は、ロボット１の原軸３が回動変位す
る時に、原軸３からハンド９までの質量に応じた慣性力
が作用し、特に回動開始時及び回動停止時にこれに抗す
る慣性力が働くため、それを相殺する軸トルクをモータ
１０に発生させるための補償指令値を出力する回路で、
オペアンプＯＰ２と入力抵抗Ｒ３，コンデンサＣ及び帰
還抵抗Ｒ４からなる微分器である。The inertia compensation circuit 62 is configured to resist the inertia force that is applied when the spindle 3 of the robot 1 rotates and displaces, depending on the mass from the spindle 3 to the hand 9, and especially when the rotation starts and stops. This is a circuit that outputs a compensation command value to cause the motor 10 to generate shaft torque that offsets the inertial force that acts.
This is a differentiator consisting of an operational amplifier OP2, an input resistor R3, a capacitor C, and a feedback resistor R4.

そして、タコジェネレータ３Ｓから第６図（イ）に示す
ような電圧信号Ｖｔｇを入力すると、慣性補償指令値と
して同図（ハ）に示すようなパルス状の電圧信号Ｖｂ　
（加速時には加速方向に、減速時には減速方向にトルク
を発生させる極性となる）を出力する。When a voltage signal Vtg as shown in FIG. 6(A) is input from the tacho generator 3S, a pulse-like voltage signal Vb as shown in FIG. 6(C) is generated as an inertia compensation command value.
(The polarity is such that torque is generated in the acceleration direction when accelerating, and in the deceleration direction when decelerating).

動摩擦補償回路６３は、ロボット１の原軸３が回動変位
する時に、軸受や駆動力伝達機構との間に発生する動摩
擦力を相殺する軸トルクをモータ１０に発生させるため
の補償指令値を出力する回路で、オペアンプＯＰ３と抵
抗Ｒ５、Ｒ６と比較電圧Ｖｒを発生させるためのボリュ
ームｖＲ４からなるヒステリシスをもった比較器である
。The dynamic friction compensation circuit 63 generates a compensation command value for causing the motor 10 to generate shaft torque that offsets the dynamic friction force generated between the bearing and the drive force transmission mechanism when the original shaft 3 of the robot 1 rotates. The output circuit is a comparator with hysteresis consisting of an operational amplifier OP3, resistors R5 and R6, and a volume vR4 for generating a comparison voltage Vr.

そして、タコジェネレータ３日か゛ら第６図（イ）に示
すような電圧信号Ｖｔｇを入力すると、動摩擦＝２３− 補償指令値として入力速度が多少増加した時から零にな
るまで、同図（ニ）に示すような矩形波の電圧信号Ｖｃ
（追従方向のトルクを発生させる極性）を出力する。Then, when a voltage signal Vtg as shown in Fig. 6 (a) is input to the tacho generator for 3 days, the dynamic friction = 23- is used as a compensation command value from the time when the input speed increases slightly until it becomes zero, as shown in the same figure (d). A rectangular wave voltage signal Vc as shown in
(Polarity that generates torque in the following direction) is output.

なお、追従方向すなわち原軸３の回転方向が反対になれ
ば、タコジエネレ〜り３日によって発生される電圧信号
Ｖｔｇの極性が□反転するので、これらの各補償回路Ｅ
ｉｌ、Ｅ１２，６３の出力信号の極−性も反転する。Note that if the following direction, that is, the rotational direction of the original shaft 3 is reversed, the polarity of the voltage signal Vtg generated by the tachoelectric energy will be reversed, so each of these compensation circuits E
The polarities of the output signals of il, E12, and 63 are also inverted.

これらの各補償回路６１，６２．６３の出力信号Ｖ　ａ
　、　Ｖ　ｂ　、　Ｖ　ｃは、それぞれボリュームＶＲ
１’。The output signal V a of each of these compensation circuits 61, 62, 63
, V b , and V c are the volumes VR
1'.

ＶＨ２、ＶＨ２によってレベル調整された後、加算回路
６４によってアナログ的に加算されて追従補償指令値Ｃ
８３として出力される。After the level is adjusted by VH2 and VH2, the addition circuit 64 adds the tracking compensation command value C in an analog manner.
It is output as 83.

加算回路６４は、オペアンプＯＰ　４と３個のボリュー
ムＶ　Ｒ＋　’、　Ｖ　Ｒ２、Ｖ　Ｒ３からの各信号を
入力する３個の入力抵抗Ｒ７、Ｒ８，Ｒ９と帰還抵抗Ｒ
，Ｏからなる非反転増幅器である。The adder circuit 64 includes an operational amplifier OP4, three input resistors R7, R8, and R9 that input signals from three volumes VR+', VR2, and VR3, and a feedback resistor R.
, O.

この加算回路６４から出力される追従補償指令値（電圧
信号）Ｃ８３を、第１図の切換スイッチ８Ｗｏを介して
加算回１４２へ入力させることにより、速度制御部３６
から出力される電流指令値Ｓ２がゼロであっても、電流
制御部３７によってこの追従補償指令値Ｃ８３に応じた
駆動電流をモータ１０に流し、追従動作時に原軸３に発
生する粘性抵抗力、慣性力、動摩擦力を相殺する軸トル
クを発生させ、略無抵抗で外力に追従して回動できるよ
うにする。By inputting the follow-up compensation command value (voltage signal) C83 output from the adder circuit 64 to the adder circuit 142 via the changeover switch 8Wo shown in FIG.
Even if the current command value S2 output from C83 is zero, the current control unit 37 supplies a drive current to the motor 10 according to the follow-up compensation command value C83 to reduce the viscous resistance force generated in the original shaft 3 during the follow-up operation. Generates shaft torque that offsets inertia force and dynamic friction force, allowing it to rotate following external force with almost no resistance.

ここで、この追従力補償回路６０の調整方法について説
明する。Here, a method for adjusting the following force compensation circuit 60 will be explained.

先ず、第１図の切換回路４６の切換スイッチ４日を接点
り側（図示と反対）に、切換スイッチ４７を接点ｄ側（
図示の状態）に切換えて、ロボット１を力抜き状態にし
たうえで１次の手順で′調整する。First, set the changeover switch 4 of the changeover circuit 46 in FIG.
After switching to the state shown in the figure) and putting the robot 1 in a relaxed state, the adjustment is performed in the first step.

（ａ）補償したい軸（この実施例では原軸３）を一定の
低速で動かし、第５図のボリュームＶＲ３１ＶＲ４を調
整して、追従力ができるだけ軽くなるように動摩擦補償
を調整する。(a) Move the axis to be compensated (original axis 3 in this embodiment) at a constant low speed, adjust the volumes VR31VR4 in FIG. 5, and adjust the dynamic friction compensation so that the following force is as light as possible.

（ｂ）原軸３を動かす速度を中速と高速にして、それぞ
れの場合の追従力に差が無くなるようにボリューム■Ｒ
Ｉによって粘性補償を調整する。(b) Set the speed of moving the original shaft 3 to medium speed and high speed, and adjust the volume ■R so that there is no difference in the following force in each case.
Adjust the viscosity compensation by I.

（ｃ）原軸３の動き始めと停止時の追従力をできるだけ
小さくするように、ボリュームＶ　Ｒ２によって慣性補
償を調整する。(c) Adjust the inertia compensation using the volume VR2 so that the following force when the original shaft 3 starts moving and stops is as small as possible.

次に、前述のようにプレイバック制御と力抜き制御の切
換えが可能な第１図の制御装置を用いて。Next, as described above, using the control device shown in FIG. 1, which is capable of switching between playback control and stress relief control.

第２図のロボット１に、コンベア１７に載って一定の速
度で移動するワーク１８のボルト１９を締め付ける作業
を行なわせる場合の動作について、第７図のフローチャ
ートも参照しながら説明する。The operation of the robot 1 shown in FIG. 2 to tighten the bolts 19 of the work 18 mounted on the conveyor 17 and moving at a constant speed will be described with reference to the flowchart shown in FIG. 7.

なお、ロボット１に以下に述べる締め付は作業を行なわ
せるために必要なティーチング作業は予めしであるもの
とする。It is assumed that the teaching work required to have the robot 1 perform the tightening work described below has been done in advance.

また、ロボット１のハンド９に取付けられたナツトラン
ナ１４は、通常は上昇限位置に蕊り、リミットスイッチ
１５はドッグ１４ｂに叩かれてオフになっている。そし
て、以下の説明では、第１図の切換回路４６における。Further, the nut runner 14 attached to the hand 9 of the robot 1 is normally at the upper limit position, and the limit switch 15 is turned off by being hit by the dog 14b. In the following description, the switching circuit 46 in FIG. 1 will be described.

切換スイッチ４８は接点ｇ側に切換えであるものとする
。It is assumed that the changeover switch 48 is switched to the contact g side.

この状態では、切換回路４６により指令値制御回路４５
のリレーコイルＬへの通電を断っているので、各切換ス
イッチＳＷ　Ｉ”ＳＷ＋＋の可動接片Ｃは全て固定接点
す側に切換わっており、原軸３゜第１．第２アーム５，
７．ハンド９を夫々回動させる各モータ４，１０．１１
等のプレイバック制御が可能である。In this state, the switching circuit 46 causes the command value control circuit 45 to
Since the relay coil L is de-energized, the movable contacts C of each changeover switch SW I"SW++ are all switched to the fixed contact side, and the original axis is 3 degrees, the first and second arms 5,
7. Each motor 4, 10.11 rotates the hand 9, respectively.
Playback control is possible.

そこで、第７図のステップｌで先ずモータ４゜１０等を
プレイバック制御して、原軸３．第１゜第２アーム５，
７等を原位置（待避位置であればどこでもよい）へ移動
させる。Therefore, in step 1 of FIG. 7, first, playback control is performed on the motors 4 and 10, and the original shaft 3. 1st ° 2nd arm 5,
Move the 7th grade to its original position (anywhere as long as it is a sheltered position).

次に、ハンド９に取り付けたナツトランナ１４のソケッ
ト１４．が、コンベア１７によって搬送されるワーク１
８上のボルト１日の移動軌跡上の予め定めた待機位置で
ある作業ポイントに位置するように、ステップ２で再び
モータ４，１０等をプレイバック制御して、第２図に示
すような所定の待機姿勢にする。Next, the socket 14 of the nut runner 14 attached to the hand 9. is the workpiece 1 conveyed by the conveyor 17
In step 2, the motors 4, 10, etc. are once again playback controlled so that the bolt above 8 is located at a work point, which is a predetermined standby position on the daily movement trajectory, and is moved to a predetermined position as shown in FIG. into the standby position.

この状態で、ボルト通過検知器２０がワーク１８上のボ
ルト１９の通過を検知するのを待ち、ボルト通過検知器
２０力ゾル１１日の通過を検知すそと、その検知信号が
第１．図の中央、処理部２！！に入力し、それによって
ステップ３の判断がＹＥＳになり、ステップ４でエアシ
リ、ンダ１３を、駆動してナツトランナー４を下畔させ
る。In this state, wait for the bolt passage detector 20 to detect the passage of the bolt 19 on the workpiece 18, and when the bolt passage detector 20 detects the passage of the bolt 11, the detection signal is the first. Center of the figure, processing section 2! ! As a result, the determination in step 3 becomes YES, and in step 4, the air cylinder and cylinder 13 are driven to move the nut runner 4 to the bottom.

、ナツトランナ、１４が下降し始めると、上昇、限り。, Natsutranna, 14 begins to fall, rises, as long as.

ミツトスイッチ１５が直ちにオンになるため、切換回路
４６により指令値制御回路４５のリレーコイルしに通電
され、その各、切換スイッチＳＷｔ〜ＳＷ、、の可動接
片Ｃが全て第１図に示すように固ゝζ１　。Since the mitsuto switch 15 is immediately turned on, the relay coil of the command value control circuit 45 is energized by the switching circuit 46, and the movable contacts C of each of the switching switches SWt to SW are all moved as shown in FIG. Fixed to ζ1.

定接、点ａ側に切換わるので、各軸の駆動用モータ４．
１０．１１等は全てフリ７にな、す１．原軸３′＆ｌび
第１．第２７−ム５，７等の可動部は外力を受ければ自
由に動く力抜き状態どなるが、重力バラ１．１１１ ンス補償回路５０からの補償指令値によってモータ４，
１１には自重によるモーメントに抗する一トルクを発生
するための駆動電流が流がされる。ため１重カバランス
が、保たｔ！工第１．第２アーム５゜７の姿勢が崩れる
ことはな、い。、１３．。Since the constant contact is switched to the point a side, the drive motor 4 for each axis.
10. All 11th prizes will be Furi 7.1. Original axis 3'&l and 1st. If the movable parts such as the 27th motors 5 and 7 are subjected to an external force, they will be in a relaxed state where they can move freely, but the gravity balance 1.111 motor 4,
A driving current is passed through 11 to generate a torque that resists the moment due to its own weight. Because of this, the single-layer balance was maintained! Engineering No. 1. The posture of the second arm 5°7 will not collapse. , 13. .

そして、ナツトランナー４が下降限ま、で下降すると、
コンベア１７によって搬送されてきたワーク、１８上の
ボルト１日をソケット１４ａが頂度衝え込！、Ｊ′うに
なり、同時に下降限りミツトスイッチ１６がドッグ１４
ｂに叩かれて作動し、その信号により、ステップ５の判
断がＹＥＳになり、ステップ６です、ットランナ１４を
駆動してソケット１．４ａ＆ｑ転させ、ボルト１日を締
め付ける。Then, when the nut runner 4 descends to the lowering limit,
The socket 14a hits the top of the bolt on the workpiece 18 transported by the conveyor 17! , J', and at the same time, the lower limit switch 16 is set to dog 14.
b is struck and activated, and based on that signal, the judgment in step 5 becomes YES, and in step 6, the runner 14 is driven to rotate the sockets 1.4a & q, and the bolts are tightened.

柔の、ように、ナツトランナ１４のソケ、ット１４ａが
ボルト１Ｓを衝え込んだ状態で締め付は作業・を行なっ
ている間もコンベア１７はワーク１８を移動させている
が、ロボット１の原軸３及び第１゜第２アーム５，７等
の各可動部は外力によって自−由に動かせる状態にある
ので、ボルト１日を締め付けながらその移動に追従して
ハンド９が水平方向に動くことができる。Similarly, while the bolt 1S is being tightened with the socket 14a of the nut runner 14 pushed in, the conveyor 17 is moving the workpiece 18. Since the movable parts such as the original shaft 3 and the first and second arms 5 and 7 can be moved freely by external force, the hand 9 moves horizontally to follow the movement while tightening the bolt. be able to.

そ９際、腰−３に作用する粘性、慣性、及び動摩擦によ
る各抵抗力は、前述の追従力補償回路６０からの追従力
、補・償指、令輝に応じてモータ１０に駆−電流が流さ
れるため相殺され、殆んど無抵抗で追従する。　　− そして、締め付けを開始してからの時間又は締付トルク
を計測することによって、ステップ７でボルトＩＳの締
め付けを終了したか否かを判定し、締め付けを終了した
らステップ８でナツトランナ１４の駆動を停止すると共
に、エアシリンダ１３を駆動してナツトランナ１４を上
昇させる。At that time, each resistance force due to viscosity, inertia, and dynamic friction acting on the waist 3 causes a drive current to be applied to the motor 10 in accordance with the following force, compensation/compensation command, and command from the above-mentioned following force compensation circuit 60. It is canceled out because it is washed away, and it follows with almost no resistance. - Then, by measuring the time from the start of tightening or the tightening torque, it is determined in step 7 whether or not tightening of the bolt IS is finished, and when the tightening is finished, the nut runner 14 is driven in step 8. At the same time, the air cylinder 13 is driven to raise the nut runner 14.

ナツトランナ１４が上昇限まで上昇すると、上昇限りミ
ツトスイッチ１５がオフになるため、指令値制御回路４
５のレリーコイルＬが非励磁になり、各切換スイッチＳ
Ｗ、〜ＳＷ＋＋は全て接点す側に切換わるので、各軸の
駆動用モータ４，１０゜１１等は全てプレイバック制御
が可能な状態に戻る。When the nut runner 14 rises to the upper limit, the upper limit switch 15 turns off, so the command value control circuit 4
5 relay coil L becomes de-energized, and each changeover switch S
Since W, ~SW++ are all switched to the contact side, the drive motors 4, 10, 11, etc. of each axis are all returned to a state in which playback control is possible.

このプレイバック制御が可能な状態に戻った時点では、
ロボット１の可動部は待機位置での状態から動いてしま
っているので、各モータ用の位置制御部には、その動い
た分の位置偏差が溜っている。When this playback control becomes possible again,
Since the movable parts of the robot 1 have moved from the standby position, positional deviations corresponding to the movements have accumulated in the position control parts for each motor.

したがって、プレイバック制御が可能な状態になると、
各可動部は直ちに動く前の作業ポイントに戻り始めるが
、上昇限りミツトスイッチ１５がオフになると、ステッ
プ９からステップ１へ戻り、再び最初の原位置へ移動さ
せる処理が行なわれるため、原軸３及び第１．第２アー
ム５，７等の各可動部は作業ポイントに戻りながら結果
的には原位置に復帰し、再び上記の動作を繰り返す。Therefore, when playback control becomes possible,
Each movable part immediately begins to return to the working point before it moved, but when the lift limit switch 15 is turned off, the process returns from step 9 to step 1, and the process of moving it to the initial original position is performed again. and 1st. Each of the movable parts such as the second arms 5, 7, etc. returns to the working point and eventually returns to its original position, and repeats the above operation again.

このようにして、ナツトランナ１４がワーク１８のボル
ト１９を締め付けている間は、ロボット１の各可動部は
力を抜いてワーク１８の移動に追従し、それによって従
来非常に煩雑な制御を行なわなければならなかった追従
作業を非常に簡単に実現できる。In this way, while the nut runner 14 is tightening the bolt 19 of the workpiece 18, each movable part of the robot 1 relaxes and follows the movement of the workpiece 18, which conventionally requires very complicated control. Follow-up work that was previously required can be accomplished very easily.

また、コンベア１７を止めて、静止しているワーク１８
上のボルト１日の締め付は作業を行なう場合は、切換回
路４６における切換スイッチ４８を接点り側に切換える
と共に、切換スイッチ４７を接点ｅ側に切換えて、ボル
ト１日の静止位置と前述の作業ポイントとを一致させて
おけば、第７図のステップ３のボルト通過の判断を省略
することによって、その静止したボルト１日の締め付け
３１一 作業を行なうことができる。In addition, the conveyor 17 is stopped and the stationary workpiece 18 is
When tightening the upper bolt for 1 day, switch the changeover switch 48 in the changeover circuit 46 to the contact side, and switch the changeover switch 47 to the contact e side to set the bolt to the rest position for 1 day and the above-mentioned position. If the work points are made coincident with each other, by omitting the determination of whether the bolt has passed in step 3 in FIG. 7, it is possible to perform the tightening 31 work of the stationary bolt for one day.

さらに、切換スイッチ４８を接点り側に切換えてあれば
、切換スイッチ４７の方を必要に応じて接点ｄ側に切換
えることによって、何時でも第１゜第２７−ム５，７等
を外力によって自由に動かし得る力抜き状態にすること
ができる。Furthermore, if the changeover switch 48 is switched to the contact side, the 1st, 27th, 5th, 7, etc. can be freely controlled by external force by switching the changeover switch 47 to the contact d side as necessary. It can be put into a relaxed state where it can be moved freely.

なお、上記実施例では、実際の速度指令値と速度フィー
ドバック値を共に位置決め制御とは無関係な零値に切り
換えることによって電流指令値を零にするようにした例
について述べたが、この他に両値を共に位置決め制御と
は無関係な互いに等しい所定値に切換えることによって
も電流指令値を零にすることができる。あるいは、電流
指令値を直接零値に切換えるようにしてもよい。In the above embodiment, an example was described in which the current command value is set to zero by switching both the actual speed command value and the speed feedback value to zero values that are unrelated to positioning control. The current command value can also be made zero by switching both values to mutually equal predetermined values that are unrelated to positioning control. Alternatively, the current command value may be directly switched to the zero value.

また、前述の実施例における重力バランス補償回路５０
では、ボテンシゴメータ３０．４０からの第３図の角度
０１＋　　０２に相当するデータに応じて、重力補償値
を演算して算出するようにしていたが、予かじめ各角度
θｌ、θ２に応じた各垂直関節軸に対する最適な重力補
償値を第４図のメモリ５２にテーブルとして格納してお
いて、久方角度データに応じてＣＰＵ５１がそのテーブ
ルがら重力補償値を読み出すようにしてもよい。Moreover, the gravity balance compensation circuit 50 in the above-mentioned embodiment
In this case, the gravity compensation value was calculated according to the data corresponding to the angles 01+02 in Fig. 3 from the potentiometer 30.40, but in advance each value corresponding to each angle θl and θ2 was calculated. The optimum gravity compensation values for the vertical joint axes may be stored as a table in the memory 52 of FIG. 4, and the CPU 51 may read out the gravity compensation values from the table in accordance with the distance angle data.

さらに、この実施例では重力バランス補償及び追従補償
をロボットの力抜き状態でのみで行なうようにしたが、
これらの補償を常時行なうようにしてもよい。Furthermore, in this embodiment, gravity balance compensation and tracking compensation are performed only when the robot is relaxed.
These compensations may be performed all the time.

また、追従力補償をその効果が最も大きい原軸に対して
のみ行なうようにしたが、周軸及び対軸等信の各軸に対
しても行なうようにすれば一層完全な補償が可能になる
。In addition, while the following force compensation is performed only on the original axis, where the effect is greatest, even more complete compensation will be possible if it is also performed on the circumferential axis and each axis of the opposite axis. .

なお、この実施例のように追従力補償として粘性補償の
他に慣性補償及びカ摩擦補償も行なうことにより略完全
な追従力補償を行なうことができるが、粘性補償のみを
行なってもかなりの効果がある。In addition, as in this example, by performing inertia compensation and friction compensation in addition to viscosity compensation as follow-up force compensation, almost complete follow-up force compensation can be achieved, but even if only viscosity compensation is performed, the effect is considerable. There is.

その場合、粘性補償指令値は可動部の変位速度に比例し
た値とするのがよい。In that case, the viscosity compensation command value is preferably a value proportional to the displacement speed of the movable part.

この発明は、第２図に示した垂直多関節型ロボットに限
らず、水平多関節型ロボットや平行リンファームを有す
る垂直多関節型ロボット等各種のロボットに適用できる
。なお、水平多関節型ロボットに適用する場合には重力
バランス補償は不要である。The present invention is applicable not only to the vertically articulated robot shown in FIG. 2, but also to various robots such as horizontally articulated robots and vertically articulated robots having a parallel link farm. Note that gravity balance compensation is not required when applied to a horizontal articulated robot.

Ｌ発明の効果〕 以」二説明してきたように、この発明によれば、速度指
令値とロボットの可動部の速度検出系からの速度フィー
ドバック値との偏差に基づく指令値に応じて、ロボット
の可能部を駆動するモータの駆動電流を制御するように
したロボットの制御装置において、速度指令値と速度フ
ィードバック値との偏差に基づく指令値を実際の速度指
令値と速度フィードバック値に拘らず零にすることによ
りロボットの可動部を外力により自由に動かし得るよう
にしたので、指令値制御手段として接点容量が小さく小
型で安価な電磁リレー等を用いることができ、それによ
って接点のメンテナンス回数を少なくできるばかりか、
接点の切換時に突入電流がモータに流れることがないた
めその防ＩＩ：、対策を施さケくて済む。[Effects of the Invention] As described above, according to the present invention, the speed of the robot is adjusted according to the command value based on the deviation between the speed command value and the speed feedback value from the speed detection system of the movable part of the robot. In a robot control device that controls the drive current of a motor that drives a movable part, the command value based on the deviation between the speed command value and the speed feedback value is set to zero regardless of the actual speed command value and speed feedback value. By doing this, the movable parts of the robot can be moved freely by external force, so it is possible to use small and inexpensive electromagnetic relays with small contact capacity as a command value control means, thereby reducing the frequency of contact maintenance. Not only,
Since no inrush current flows to the motor when the contacts are switched, there is no need to take preventive measures.

また、外力による可動部の変位時に、その関節軸に発生
する粘性抵抗に対応する軸トルクを発生させるようにそ
の関節軸を駆動するモータの駆動電流を制御して粘性抵
抗を相殺するようにしたので、ロボットの追従動作時に
、その可動部を殆んど無抵抗で軽く動かせるためスムー
ズな追従動作ができ、ハンドに取付けた工具がワークか
ら外れるようなことがなくなる。In addition, when the movable part is displaced by an external force, the drive current of the motor that drives the joint axis is controlled to generate a shaft torque corresponding to the viscous resistance generated in the joint axis, thereby canceling out the viscous resistance. Therefore, when the robot performs a follow-up operation, its movable parts can be moved lightly with almost no resistance, allowing smooth follow-up operation and preventing the tool attached to the hand from coming off the workpiece.

【図面の簡単な説明】[Brief explanation of the drawing]

第１図はこの発明の一実施例を示す制御装置のブロック
構成図、 第２図はこの発明を適用する垂直多関節型ロボットの構
成及びその作業の説明に供するロボットまわりの外観図
。 第３図は同じくそのロボットの垂直関節軸に加わる重力
のモーメントを説明するための模式第４図は第１図にお
ける重力バランス補償回路５０の具体例を示すブロック
回路図、 第５図は第１図における追従力補償回路６０の具３５一 体側を示す回路図、 第６図は同じくその作用説明のための各部の信号波形図
、　　　　　　　　　　　　　。 第７図は第１図における中央処理装置２３の動作例を示
すフロー図である。 １・・・垂直多関節型ロボット　、３・・・原軸　。 ４．１０．１１・・・ＤＣサーボモータ５・・・第１ア
ーム　、６・・・対軸　　７・・・第２アーム８・・・
手首軸　　　　９・・・ハンド　　。 １条・・・エアシリンダ　　　１４・・・ナツトランナ
Ｉｓ、ＩＥｉ・・・リミットスイッｔ １７・・・コンティニュアスコンづア １８・・・ワ、−り　　　　１Ｂ・・・ボルト　　、５
２０・・・ボルト通過検知器　　２３・・・中央処理部
２９．３９・・・タコジェネレータ ３０．４０・・・ポテンショメータ ４５・・・指令値制御回路　　　４６・・・切換回路５
０・・・重力バランス補償回路　　　　　　。 ６０・・・追従力補償回路　　６１・・・粘性婦！回路
６２・・・慣性補償回路、　、６３・・・動亭擦補償回
路６４・・・加算回路 番 第７図 スタート ■　原位置へ移動 作業ポイントへ ■　　　　　移　　動 ■　　ボルト通過？ ■　　ナットラノナ下降FIG. 1 is a block configuration diagram of a control device showing an embodiment of the present invention, and FIG. 2 is an external view of the surroundings of the robot for explaining the configuration and work of a vertically articulated robot to which the present invention is applied. FIG. 3 is a schematic diagram for explaining the moment of gravity applied to the vertical joint axes of the robot. FIG. 4 is a block circuit diagram showing a specific example of the gravity balance compensation circuit 50 in FIG. 1. FIG. 6 is a circuit diagram showing the integrated side of the tool 35 of the following force compensation circuit 60 in the figure, and FIG. 6 is a signal waveform diagram of each part for explaining its operation. FIG. 7 is a flow diagram showing an example of the operation of the central processing unit 23 in FIG. 1... Vertical articulated robot, 3... Original axis. 4.10.11... DC servo motor 5... First arm, 6... Twin shaft 7... Second arm 8...
Wrist axis 9...hand. Article 1... Air cylinder 14... Nut runner Is, IEi... Limit switch 17... Continuous controller 18... War, -ri 1B... Bolt, 5
20... Bolt passage detector 23... Central processing unit 29.39... Tacho generator 30.40... Potentiometer 45... Command value control circuit 46... Switching circuit 5
0...Gravity balance compensation circuit. 60...Following force compensation circuit 61...Viscous woman! Circuit 62... Inertia compensation circuit, , 63... Motion friction compensation circuit 64... Addition circuit number Figure 7 Start ■ Move to original position To work point ■ Move ■ Bolt passing? ■ Natlanona descent

Claims (1)

【特許請求の範囲】 １　速度指令値とロボットの可動部の速度検出系からの
速度フィードバック値との偏差に基づく指令値に応じて
、前記ロボットの各可動部を駆動するモータの駆動電流
を制御するようにしたロボットの制御装置において、 前記速度指令値と速度フィードバック値との偏差に基づ
く指令値を実際の速度指令値と速度フィードバック値に
拘らず零にする指令値制御回路と、この指令値制御回路
の作動・不作動を切換える切換回路と、 前記可動部の変位時に該可動部の関節軸に生ずる粘性抵
抗に対応する軸トルクを発生させるための粘性補償指令
値を出力する粘性補償回路とを設け、 少くとも前記切換回路によって前記指令値制御回路を作
動させて前記偏差に基づく指令値を零にした時、前記粘
性補償回路が出力する粘性補償指令値によって前記関節
軸を駆動するモータの駆動電流を制御し、前記ロボット
の可動部を外力により自由に動かし得るようにしたこと
を特徴とするロボットの制御装置。 ２　粘性補償回路が、可動部の変位速度に応じた粘性補
償指令値を出力する回路である特許請求の範囲第１項記
載のロボットの制御装置。[Scope of Claims] 1. Controlling the drive current of a motor that drives each movable part of the robot according to a command value based on a deviation between a speed command value and a speed feedback value from a speed detection system of a movable part of the robot. A control device for a robot configured to include a command value control circuit that makes a command value based on the deviation between the speed command value and the speed feedback value zero regardless of the actual speed command value and the speed feedback value; a switching circuit that switches between activation and inactivation of the control circuit; and a viscosity compensation circuit that outputs a viscosity compensation command value for generating a shaft torque corresponding to viscous resistance generated in the joint axis of the movable part when the movable part is displaced. and when the command value control circuit is operated by at least the switching circuit to make the command value based on the deviation zero, the motor driving the joint shaft is controlled by the viscosity compensation command value output from the viscosity compensation circuit. A control device for a robot, characterized in that a drive current is controlled so that movable parts of the robot can be freely moved by external force. 2. The robot control device according to claim 1, wherein the viscosity compensation circuit is a circuit that outputs a viscosity compensation command value according to the displacement speed of the movable part.