JPS61243513A - Robot controller - Google Patents
Robot controllerInfo
- Publication number
- JPS61243513A JPS61243513A JP8463785A JP8463785A JPS61243513A JP S61243513 A JPS61243513 A JP S61243513A JP 8463785 A JP8463785 A JP 8463785A JP 8463785 A JP8463785 A JP 8463785A JP S61243513 A JPS61243513 A JP S61243513A
- Authority
- JP
- Japan
- Prior art keywords
- command value
- speed
- circuit
- compensation
- motor
- Prior art date
- Legal status (The legal status 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 status listed.)
- Pending
Links
Landscapes
- Control Of Position Or Direction (AREA)
- Manipulator (AREA)
Abstract
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.
近年、各種の産業用ロボットが工場の製造ラインで使用
されるようになり、組立（アッセンブリ）ロボットも実
用化されつつある。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 robot is in a "relaxed state" in which movable parts such as arms can be moved freely by external force while performing a certain task, it is possible to make the robot follow the movement of the workpiece on the conveyor without performing 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 Patent Laid-Open No. 5B-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 inrush current flowing into the motor when it is off, and the need to perform frequent 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 compact and inexpensive relay switch with small contact capacity can be used as the switching control means for zeroing the command value based on the deviation, and an inrush current is generated in the motor when switching the contacts. Since it does not flow, there is no need to take measures to prevent it, and the number of times the contacts need to be maintained is reduced, so the above-mentioned problems can be solved.
しかしながら、このようなロボットの制御装置において
も、結果としてはロボットの可動部の各軸を駆動するモ
ータの駆動電流を零にして力抜き状態にするだけであっ
たため、垂直関節軸を有するロボットでは可動部が自重
によって回動してしまって姿勢を保てなくなり、力抜き
状態にしても姿勢を保てる水平多関節型ロボットでも、
その状態で外力による追従動作を行なわせると、外力に
よるロボットの可動部の変位に対してその可動部及び関
節軸（駆動力伝達部や軸受部を含む）に慣性、粘性、静
摩擦及び動摩擦等による各種の抵抗力が生ずるため、可
動部の追従力を零又はそれに近くすることができず、ま
た、ハンドにワークに対する若干の押付は力を与えて追
従作業中の係合を確実にしたり、作業終了時にハンドを
ワークから離脱させる方向に付勢力を与えて無理なく退
避させたりすることができないという問題点があった。However, even in such a robot control device, the result is that the drive current of the motors that drive each axis of the robot's movable parts is reduced to zero, and the robot is in a relaxed state. Even horizontally articulated robots that can maintain their posture even when relaxed are unable to maintain their posture because their movable parts rotate under their own weight.
When a follow-up motion is performed using an external force in this state, the displacement of the movable part of the robot due to the external force is caused by inertia, viscosity, static friction, dynamic friction, etc. Since various resistance forces are generated, it is impossible to make the tracking force of the movable part zero or close to zero, and if the hand is slightly pressed against the workpiece, force is applied to ensure engagement during tracking work, or the work There has been a problem in that it is not possible to apply a biasing force in the direction of detaching the hand from the workpiece at the end of the process, so that the hand can be easily evacuated from the workpiece.
この発明は、これらの問題点を解決して、ロボットの力
抜き制御時に外力によるスムーズな追従動作及び作業終
了時のスムーズな退避動作を可能にすることを目的とす
る。The object of the present invention is to solve these problems and enable a robot to smoothly follow an external force when controlling the robot to release its force, and to perform a smooth retraction operation at the end of the work.
そのため、この発明によるロボットの制御装置は、速度
指令値と速度フィードバック値との偏差に基づく指令値
を実際値に拘らず零にすることによって可動部の力抜き
制御を行なうと共に、その可動部に一定方向の付勢力を
付与するように、該可動部を駆動するモータに所要のト
ルク詮発生させるための補償指令値を出力する補償回路
と、上記力抜き制御時に、その補償指令値によって上記
可動部を駆動するモータの駆動電流を制御することによ
る該可動部の移動速度を制限する速度制限回路とを設け
たものである。Therefore, the robot control device according to the present invention controls the movable part to release force 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 movable part to release the force. A compensation circuit outputs a compensation command value for causing a motor that drives the movable part to generate a required torque so as to apply a biasing force in a certain direction; A speed limiting circuit is provided to limit the moving speed of the movable part by controlling the drive current of the motor that drives the movable part.
以下、この発明の実施例を添付図面を参照しながら説明
する。Embodiments of the present invention will be described below 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 waist axis 3 that stands vertically on a base 2 that is fixed on a pedestal (not shown), and also serves as a shoulder axis that is fixed at right angles to this waist axis. 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 9 is rotatably connected to the wrist shaft 8 by a wrist shaft 8.
これを模式的に示すと第３図のようになる。This is schematically shown in FIG. 3.
腰軸３は、モータ１０によって水平面内で矢示Ａ方向に
回転される水平関節軸である。The waist shaft 3 is a horizontal joint shaft that is rotated in the direction of arrow A in a horizontal plane by the motor 10.
第１アーム５は、モータ４によって矢示Ｂ方向に、第２
アーム７はモータ１１によって矢示Ｃ方向に、ハンド９
は第２アーム７に内蔵された図示しないモータによって
矢示り方向にそれぞれ垂直面内で回動し、これらを連結
する肩軸と対軸６及び手首軸８が垂直関節軸である。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 shoulder axis, the opposite axis 6, and the wrist axis 8 that connect these are vertical joint axes.
なお、モータ４，１０，１１及びハンド９を回動させる
図示しない手首軸駆動モータとしては、いずれもＤＣザ
ーボモータを使用する。そして、これらの各モータの駆
動力を伝達する減速機としては、逆伝達効率の比較的高
いもの（例えばベベルギヤ機構によるもの）を使用する
。Note that as the wrist shaft drive motor (not shown) for rotating the motors 4, 10, 11 and the hand 9, DC servo motors are used. 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 nut runner 14 attached to a holder 12 connected to the wrist shaft 8 via a force sensor 13 (details of which will be described later) and having a socket 14a at its tip for tightening a bolt.
一方、１７はコンティニュアスコンベア（以下単に「コ
ンベア」と云う）であり、所要位置に所定の姿勢で位置
決め固定した例えばエンジンブロック等のワーク１８を
載せて、ロボット１のワーキングエリア内を矢示Ｅ方向
に所定の速度で搬送するようになっている。On the other hand, reference numeral 17 denotes a continuous conveyor (hereinafter simply referred to as "conveyor"), on which a workpiece 18 such as an engine block, which is positioned and fixed at a predetermined position and in a predetermined posture, is placed, and the workpiece 18 is moved within the working 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
8 is set with a bolt to be worked on (for example, a bolt for fixing a head cover to a cylinder head), and the task of tightening this bolt is carried out by the robot 1, which has a nut runner 14 attached to the hand 9. 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
When the bolt 19 on the work 18 conveyed by the bolt 7 passes, it is detected.
なお、このボルト通過検知器２０としては、例えば反射
型の光電スイッチなどを用いる。Note that as this bolt passage detector 20, for example, a reflective photoelectric switch or the like is used.
また、ボルト通過検知器２０の代りに、ボルト１日に対
応するコンベア１７の所要位置にドッグを設けておき、
そのドッグによってコンベア１７に沿う所要位置の固定
部に取り付けたリミットスイッチを叩くようにして、こ
のリミットスイッチのオンによってボルト１９の通過を
検知するようにしても良い。Also, instead of the bolt passage detector 20, a dog is provided at a required position on the conveyor 17 corresponding to the bolt 1st,
The dog may strike a limit switch attached to a fixed portion at a predetermined position along the conveyor 17, and passage of the bolt 19 may be detected by turning on the limit switch.
力センサ１３は、例えば第４図に示すように、円板状の
取付板１３１に小径のストッパ保持筒１３２を介して大
径のセンサ保持筒１３３が固設され、センサ保持筒１３
３に十字形のセンサ取付板１３４の各端部を固着し、そ
のセンサ取付板１２４の中心部に検出軸１３５を垂直に
慣通させて固着している。For example, as shown in FIG. 4, the force sensor 13 has a large-diameter sensor holding tube 133 fixed to a disk-shaped mounting plate 131 via a small-diameter stopper holding tube 132.
3, each end of a cross-shaped sensor mounting plate 134 is fixed to the sensor mounting plate 124, and a detection shaft 135 is vertically passed through the center of the sensor mounting plate 124 and fixed thereto.
センサ取付板１３４には４個のセンサ素子（例えば半導
体ストレインケージ等の歪センサ素子）１３６を貼着し
ており、ストッパ保持筒１３２には一対のストッパボル
ト１３７が螺入して、検出軸１３５の傾斜角を規制して
いる。Four sensor elements (for example, strain sensor elements such as semiconductor strain cages) 136 are attached to the sensor mounting plate 134, and a pair of stopper bolts 137 are screwed into the stopper holding cylinder 132 to connect the detection shaft 135. The angle of inclination is regulated.
この力センサ１３は、センサ保持筒１３３に対して検出
軸１３５が外力によって傾斜されると、センサ取付板１
３４が歪むため、その歪量に応じて各センサ素子１３６
の抵抗値が変化する。When the detection shaft 135 is tilted with respect to the sensor holding cylinder 133 by an external force, the force sensor 13
34 is distorted, each sensor element 136 is distorted according to the amount of distortion.
resistance value changes.
したがって、この力センサ１３の取付板１３１を第２図
の手首軸８に連結されたホルダ１２に固着し、検出軸１
３５をナツトランナ１４の基部に固着しておけば、ナツ
トランナ１４が外力（この例ではコンベア１７によるワ
ーク１８の移動力）によって移動されてロボット１の各
可動部が追従動作する時に、その可動部が受ける追従力
の大きさに応じて検出軸が傾斜するため、その追従力の
大きさをセンサ素子１３６の抵抗値変化として検出する
ことができる。Therefore, the mounting plate 131 of this force sensor 13 is fixed to the holder 12 connected to the wrist shaft 8 in FIG.
35 is fixed to the base of the nut runner 14, when the nut runner 14 is moved by an external force (in this example, the force of moving the workpiece 18 by the conveyor 17) and each movable part of the robot 1 performs a follow-up operation, the movable part Since the detection axis is tilted according to the magnitude of the following force received, the magnitude of the following force can be detected as a change in the resistance value of the sensor element 136.
次に、第１図を参照して第２図に示したロボット１の制
御装置の実施例を説明する。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 shoulder shaft for rotating the 17th arm 5, which is configured by a current control unit 27 and the like, and a position command register 34° position control unit, which is exactly like this servo control unit. 35. A servo control section for the motor 10 that drives the waist shaft 3, which is configured by a speed control section 3, a current control section 37, etc., and a servo control section for the motor 10 that drives the waist shaft 3, which is also configured similarly to these servo control sections, although not shown in the figure. In addition to controlling the servo control unit for the paired shaft drive motor 11 that rotates the two arms 7 and the servo control unit for the wrist shaft drive motor, the nut runner 14 attached to the hand 9 shown in FIG. The built-in motor also controls the rotation and stopping of the socket 14a.
次に、モータ４用のサーボ制御部において、位置レジス
タ２４には、中央処理部２３からの第１アーム５の目標
位置指令値が逐次更新されながら書き込まれる。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 # cylinder part 25 receives the target position command value of the first arm 5 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. The value converted into a digital value by the /D converter 31, that is, the first arm 5
The speed command value Sa is output based on the deviation from the current position value (corresponding to the angle θ in Fig. 3), and the speed command value Sa is output every time the target position command value and the current position value match and positioning is completed. The central processing unit 23 is informed of this, and the central processing unit 23 uses this information 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 feedback value Vtg 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 2nd.
1 is inputted through the adder circuit 32, and a drive current is passed through the motor 4 based on the deviation from the current feedback value from the current detector 28 that detects the drive current flowing through the motor 4.
したがって、位置指令レジスタ２４９位置制御部２５．
速度制御部２日、及び電流制御部２７等からなるモータ
４用のサーボ制御部は、後述する指令値制御回路４５が
位置制御部２５からの速度指令値とタコジェネレータ２
日からの速度フィードパツク値とをそのまま速度制御部
２日に出力している限りにおいては、中央処理部２３か
らの目標位置指令値に基づいてモータ４を駆動して、第
１アーム５をプレイバック制御（位置決め制御）するこ
とができる。Therefore, position command register 249 position control section 25.
In the servo control section for the motor 4, which includes a speed control section 2, a current control section 27, etc., a command value control circuit 45, which will be described later, uses the speed command value from the position control section 25 and the tacho generator 2.
As long as the speed feed pack value from 1 is outputted as it is to the speed control section 2, the motor 4 is driven based on the target position command value from the central processing section 23, and the first arm 5 is operated. Back control (positioning control) is possible.
腰軸３を回転駆動するモータ１０用のサーボ制御部を構
成する各部３４〜４２も、上述したモータ４用のサーボ
制御部を構成する各部２４〜３２と全く同様に機能し、
やはり後述する指令値制御回路４５が位置制御部３５か
ら出力される速度指令値ｓｂとモータ１０の出力軸に取
付けられたタコジェネレータ３Ｓから出力される速度フ
ィードバック値とをそのまま速度制御部３６に出力して
いる限りにおいては、中央処理部２３からの目標位置指
令値に基づいてモータ１０を駆動して腰軸３及びそれと
一体のモータ４の向きをプレイバック制御（位置決め制
御）することができる。The parts 34 to 42 forming the servo control part for the motor 10 that rotationally drives the waist shaft 3 function in exactly the same way as the parts 24 to 32 forming the servo control part for the motor 4 described above.
A command value control circuit 45, which will also be described later, outputs the speed command value sb output from the position control section 35 and the speed feedback value output from the tacho generator 3S attached to the output shaft of the motor 10 as they are to the speed control section 36. As long as it is, the motor 10 can be driven based on the target position command value from the central processing section 23 to perform playback control (positioning control) of the direction of the waist shaft 3 and the motor 4 integrated therewith.
さらに、第２アーム７を駆動するモータ１１用及び手首
軸駆動モータ用の各サーボ制御部も、同様に機能して夫
々各モータを駆動し、第２アーム７及びハンド９をプレ
イバック制御することができる。Furthermore, the servo control units for the motor 11 that drives the second arm 7 and the wrist shaft drive motor function similarly to drive each motor, respectively, and perform playback control of the second arm 7 and hand 9. Can be done.
指令値制御回路４５は、リレーコイルＬ１の励磁・非励
磁によって切換わる１１個の切換スイッチＳ Ｗ 、〜
ＳＷ＋＋（モータ１１用及び手首軸駆動モータ用サーボ
制御部に介挿する４個の切換スイッチＳＷ５〜ＳＷ８は
図示していない）を有する電磁リレーからなる。The command value control circuit 45 includes 11 changeover switches SW, ~, which are switched by energizing/de-energizing the relay coil L1.
It consists of an electromagnetic relay having SW++ (four changeover switches SW5 to SW8 inserted in the servo control section for the motor 11 and the wrist shaft drive motor are not shown).
この指令値制御回路４５の切換スイッチＳＷ１゜ＳＷ２
の可動接片Ｃは夫々速度制御部２６の入力側に接続され
、各固定接点ａは後述する速度制限回路に、各固定接点
すは位置制御部２５の出力側及びタコジェネレータ２日
に夫々接続されている。Changeover switch SW1゜SW2 of this command value control circuit 45
The movable contacts C are respectively connected to the input side of the speed control unit 26, and each fixed contact a is connected to the speed limiting circuit described later, and each fixed contact is connected to the output side of the position control unit 25 and the tachogenerator 2, respectively. has been done.
また、切換スイッチ５Ｗ３１ ＳＷ４の可動接片Ｃは夫
々速度制御部３６の入力側に接続され、各固定接点ａは
アースに、各固定接点すは位置検出部３５の出力側及び
タコジェネレータ３９に夫々接続されている。Furthermore, the movable contacts C of the changeover switches 5W31 SW4 are each connected to the input side of the speed control unit 36, each fixed contact a is connected to the ground, and each fixed contact is connected to the output side of the position detection unit 35 and the tachogenerator 39, respectively. It is connected.
図示しない切換スイッチＳＷ５．ＳＷ６及びＳＷ ？
ｔ Ｓ Ｗ ６も、夫々モータ１１用のサーボ制御部及
び手首軸駆動モータ用サーボ制御部において、全く同様
に接続されている。Selector switch SW5 (not shown). SW6 and SW?
t SW 6 is also connected in exactly the same way in the servo control section for the motor 11 and the servo control section for the wrist shaft drive motor, respectively.
切換スイッチＳ Ｗ ｓ ＋ Ｓ Ｗ ｔｏは、後述す
る重力バランス補償回路５０から出力される重力補償指
令値Ｃ８１，Ｃ８２をそれぞれ加算回路３２及びモータ
１１用の図示しないサーボ制御部における同様な加算回
路に入力するラインに介挿されて、開閉スイッチとして
用いられている。The changeover switch S W s + S W to transfers gravity compensation command values C81 and C82 outputted from a gravity balance compensation circuit 50, which will be described later, to an addition circuit 32 and a similar addition circuit in a servo control section (not shown) for the motor 11, respectively. It is inserted into the input line and used as an on/off switch.
さらに、切換スイッチＳＷ＋＋は、後述する追従力補償
回路６０から出力される追従力補償指令値Ｃ８３を加算
回路４２に入力するラインに介挿されて、開閉スイッチ
として用いられている。Further, the changeover switch SW++ 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 addition circuit 42, and is used as an opening/closing switch.
なお、リレーコイルＬ１の両端に接続したダイオードＤ
１はフライホイールダイオードである。In addition, a diode D connected to both ends of the relay coil L1
1 is a flywheel diode.
この指令値制御回路４５は、リレーコイルＬ１の非励磁
時には、各切換スイッチＳＷ、〜ＳＷ＋＋の可動接片Ｃ
が固定接点す側に夫々切換わっていて、実際の速度指令
値と速度フィードバック値をそのまま通過させて、各モ
ータ用のサーボ制御部を本来どおり位置決め動作させる
が、リレーコイルＬ１が励磁されると、各切換スイッチ
ＳＷＩ〜ＳＷ＋＋の可動接片Ｃが図示のように固定接点
ａ側に切換わり、各モータ用のサーボ制御部における速
度制御部２６．！＋６．・・・に入力する速度指令値と
速度フィードバック値をいずれも零（アース値）にして
、その偏差に基づく電流指令値８１＊Ｓ２＋・・・を実
際の速度指令値と速度フィードバック値の偏差にかかわ
らず、換言するとモータ４，１０等の動きに関係なくゼ
ロにする。This command value control circuit 45 controls the movable contacts C of each changeover switch SW, ~SW++ when the relay coil L1 is not energized.
are switched to the fixed contact side, allowing the actual speed command value and speed feedback value to pass through as they are, and positioning the servo control section for each motor as originally intended. However, when relay coil L1 is energized, , the movable contact piece C of each changeover switch SWI to SW++ is switched to the fixed contact a side as shown in the figure, and the speed control section 26. in the servo control section for each motor. ! +6. Set both the speed command value and speed feedback value input to 0 (earth value), and use the current command value 81*S2+... based on the deviation as the deviation between the actual speed command value and speed feedback value. In other words, it is set to zero regardless of the movement of the motors 4, 10, etc.
このように、電流指令値を実際の速度指令値と速度フィ
ードバック値にかかわらずゼロにすると、位置及び速度
フィードバック制御が効かなくなるため、各モータ４，
１０．１１等はフリーの状態になり、それによって第２
図のロボット１の原軸３、第１アーム５．第２アーム７
、及びハンド９は外力によって自由に動かせるようにな
る。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 no longer be effective, 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. 2nd arm 7
, 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 S causes each vertical joint axis to rotate, causing the postures of these movable parts to collapse and making it impossible to perform the work, so a gravity balance compensation circuit 50 is provided. , thereby maintaining the gravity balance to prevent the posture from collapsing and applying a biasing force in a certain direction to the movable part, the details of which will be described later.
また、水平関節軸である原軸３を駆動するモータ１０の
サーボ制御部に対して、追従動作時における追従力補償
を行なうための追従力補償回路６０を設けているが、そ
の詳細も後述する。Further, a following force compensation circuit 60 is provided for the servo control unit of the motor 10 that drives the original shaft 3, which is the horizontal joint axis, to compensate for the following force during the following operation, and the details thereof will be described later. .
なお、指令値制御回路４５の各切換スイッチＳＷ１〜Ｓ
Ｗｕの可動接片Ｃが固定接点す側に切り換わっている時
でも、位置決め完了時には速度指令値と速度フィードバ
ック値の偏差が零になるが、この時には位置及び速度フ
ィードバック制御が効いているため、原軸３．第１．第
２アーム５゜７等に外力が加わってモータ１０，４．１
１等が停止位置から回転されると、直ちにそれを元に戻
そうとする回転力が発生する。In addition, each changeover switch SW1 to S of the command value control circuit 45
Even when the movable contact piece C of Wu 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 Original axis 3. 1st. When an external force is applied to the second arm 5゜7 etc., the motor 10, 4.1
When the 1st class is rotated from its rest position, a rotational force is immediately generated to return it to its original position.
４６は切換回路であり、電源Ｖｃｃの給電回路に介挿し
た切換スイッチ４７．４８と、中央処理部２３からの制
御信号Ｏｓによってオン・オフ制御されるスイッチング
トランジスタ４９とによって構成されている。Reference numeral 46 denotes a switching circuit, which is composed of changeover switches 47 and 48 inserted into the power supply circuit of the power supply Vcc, and a switching transistor 49 that is turned on and off by a control signal Os from the central processing section 23.
この切換回路４６は、図示のように切換スイッチ４８の
可動接片ｉを固定接点ｇ側に切換えておくと、通常の動
作時は中央処理部２３がらの制御信号ＯｓがＬ″になっ
ているため、トランジスタ４日がオフになっており、指
令値制御回路４５のリレーコイルＬ１を非励磁にしてい
る。そして、力抜き制御を行なう時に制御信号ＯｓがＨ
″になり、トランジスタ４日をオンにしてリレーコイル
Ｌ１を励磁する。In this switching circuit 46, when the movable contact i of the changeover switch 48 is switched to the fixed contact g side as shown in the figure, the control signal Os from the central processing unit 23 is set to L'' during normal operation. Therefore, the transistor 4 is turned off, and the relay coil L1 of the command value control circuit 45 is de-energized.Then, when performing strain relief control, the control signal Os is set to H.
'', transistor 4 is turned on and relay coil L1 is energized.
また、切換スイッチ４８の可動接片ｉを固定接点り側に
切換えておくと、トランジスタ４日のオン・オフにかか
わらず、切換スイッチ４７によって指令値制御回路４５
の作動・不作動を切換えることができる。If the movable contact i of the changeover switch 48 is switched to the fixed contact side, the changeover switch 47 will control the command value control circuit 45 regardless of whether the transistor 4 is on or off.
can be switched between activation and deactivation.
なお、この実施例においては、リレーコイルＬ１の励磁
と指令値制御回路４５の作動とが対応しているが、切換
スイッチＳＷ１〜５Ｗ１１の固定接点ａとｂの接続を逆
にすれば、リレーコイルＬ１の非励磁と指令値制御回路
４５の動作とを対応させるようにすることも可能である
。In this embodiment, the excitation of the relay coil L1 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 SW1 to SW11 are reversed, the relay coil It is also possible to make the de-energization of L1 correspond to the operation of the command value control circuit 45.
次に、重力バランス補償回路５０の具体例を第５図によ
って説明する。Next, a specific example of the gravity balance compensation circuit 50 will be explained with reference to FIG.
この重力バランス補償回路５０は、ＣＰＵ （中央処理
装置）５１とプログラムメモリとしてのＲＯＭ及びデー
タメモリとしてのＲＡＭを含むメモＩＪ５２と１対ずつ
のＡ／Ｄ変換器５３．５４及びＤ／Ａ変換器５５．５６
とからなるマイクロコンピュータと、アンバランス付与
回路５７．５８（その詳細は後述する）とによって構成
されている。This gravity balance compensation circuit 50 includes a CPU (central processing unit) 51, a memory IJ52 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. 55.56
and unbalance imparting circuits 57 and 58 (details of which will be described later).
そして、この重力バランス補償回路５０は、モータ４の
出力軸に取付けられたポテンショメータ３０から出力さ
れる第１アーム５の水平位置からの回動角度θ１ （第
３図参照）に相当する電圧信号をＡ／Ｄ変換器５３によ
ってデジタル値に変換してｃＰＵ５ｉに読込み、同様に
モータ１１の出力軸に取付けられたポテンショメータ４
日から出力される第１アーム５に対する第２アーム７の
回動角度θ２ （第３図参照）に相当する電圧信号をＡ
／Ｄ変換器５４によってデジタル値に変換してＣＰＵ５
ｉに読込む。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 5i, 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.
Load into i.
そして、メモリ５２に予めロボット１の第１アーム５の
肘軸４ａから重心までの長さ１１及び全長ｐ２、第２ア
ーム７の対軸６から重心までの長さ１２３及び全長１４
、第１アーム５．第２アーム７及びハンド９の各重量Ｗ
ｒ ｒ Ｗ ２ ｐ Ｗ ａ （第３図参照）、及び
ｓｉｎθ、 ｃｏｓθのテーブルを記憶させておき、垂
直関節軸である肘軸４ａ及び対軸６における重力のモー
メントＭ（Ａ）、Ｍ（’Ｂ）をＣＰＵ５１が下記の演算
を行なって算出し、それに抗し得る軸トルクを発生させ
るための補償指令値を出力する。The length 11 and total length p2 of the first arm 5 of the robot 1 from the elbow axis 4a to the center of gravity are stored in the memory 52 in advance, and the length 123 and total length 14 of the second arm 7 from the opposite axis 6 to the center of gravity are stored in advance.
, first arm5. Each weight W of the second arm 7 and hand 9
r r W 2 p W a (see Figure 3), and tables of sin θ and cos θ are stored, and the moments of gravity M(A) and M(' The CPU 51 calculates B) by performing the following calculation, and outputs a compensation command value for generating shaft torque capable of resisting it.
Ｍ（Ａ）＝に＋ ｃｏｓＯｌ ＋に２ ｓｊ、ｎ（θ１
＋０２−９０°）Ｍ（Ｂ）＝ Ｋ ２ ｓｉｎ （θ、
十ｆＪ２−９０°）但し、Ｋ １＝１２１Ｗ＋ ＋Ｉ
２ （Ｗ２ ＋Ｗ３ ）Ｋ２ ＝Ａ’ ３Ｗ２ ＋Ａ’
４ Ｗ３で与えられる。このＫ １ ｒ Ｋ ２は定
数であるから、これを予めメモリ５２に格納しておくと
よい。M(A) = to + cosOl + to 2 sj, n(θ1
+02-90°)M(B)=K2sin(θ,
(10fJ2-90°) However, K 1=121W+ +I
2 (W2 +W3)K2 =A' 3W2 +A'
4 Given by W3. Since K 1 r K 2 is a constant, it is preferable to store it in the memory 52 in advance.
この重力バランス補償回路５０におけるＣＰＵ５１の動
作フローを第６図に示す。The operation flow of the CPU 51 in this gravity balance compensation circuit 50 is shown in FIG.
このようにしてＣＰＵ５１が肘軸４ａ及び対軸６ｔこお
ける第１．第２アーム５，７及びハンドＳの自重による
重力のモーメントに対応する（バランスする）軸トルク
を発生させるための補償指令値Ｃ８ａ、Ｃ８ｂを算出し
て出力するが、これをそれぞれアンバランス付与回路５
７．５８を通して上記の重力モーメントに対して若干ア
ンバランスな所要のトルクを発生させて第１７−ム５に
一定方向の付勢力を与えるようにして、Ｄ／Ａ変換器５
５．５６によってそれぞれアナログ信号（電圧信号）に
変換して補償指令値Ｃ８０，Ｃ８２として出力する。In this way, the CPU 51 operates the first shaft 4a and the opposite shaft 6t. Compensation command values C8a and C8b are calculated and outputted to generate shaft torques that correspond to (balance) the moment of gravity due to the weight of the second arms 5 and 7 and the hand S, and these are sent to the respective unbalance imparting circuits. 5
Through 7.58, a required torque that is slightly unbalanced with respect to the above-mentioned gravitational moment is generated to apply a biasing force in a certain direction to the 17th arm 5, and the D/A converter 5
5.56, each is converted into an analog signal (voltage signal) and output as compensation command values C80 and C82.
アンバランス付与回路５７は、予め設定した付与すべき
アンバランス力（付勢力）に応じたデジタル値ΔＳａを
出力するアンバランス力設定器（例えばボリュームとＡ
／Ｄ変換器からなる）５７１と、加算器５７２及び減算
器５７３と、データセレクタ５７４及びそのセレクト信
号を切換えるためのスイッチ５７５等によって構成され
ている。The unbalance applying circuit 57 includes an unbalance force setting device (for example, a volume and an
571 (consisting of a /D converter), an adder 572 and a subtracter 573, a data selector 574 and a switch 575 for switching its select signal.
そして、加算器５７２はＣＰＵ５１からの補償指令値Ｃ
８ａにアンバランス力設定器５７１からのデジタル値Δ
Ｓａを加算してＣ８ａ＋ΔＳａを出力する。一方、減算
器５７３はＣＰＵ５１からの補償指令値Ｃ８ａからデジ
タル値ΔＳａを減算してＣ３ａ−ΔＳａを出力する。Then, the adder 572 receives the compensation command value C from the CPU 51.
8a is the digital value Δ from the unbalance force setting device 571.
Sa is added and C8a+ΔSa is output. On the other hand, the subtracter 573 subtracts the digital value ΔSa from the compensation command value C8a from the CPU 51 and outputs C3a-ΔSa.
データセレクタ５７４は、例えばスイッチ５７５がオフ
でセレクト信号ＳｃがＨ″ （Ｖｃｃ電位）になってい
ると、減算器５７３からのデータをそのままＤ／Ａ変換
器５５へ出力し、スイッチ５７５がオンになってセレク
ト信号ＳＣがＬ″（アース電位）になっていると、加算
器５７２からのデータをそのままＤ／Ａ変換器５５へ出
力する。For example, when the switch 575 is off and the select signal Sc is at H'' (Vcc potential), the data selector 574 outputs the data from the subtracter 573 as is to the D/A converter 55, and the switch 575 is turned on. When the select signal SC is at L'' (earth potential), the data from the adder 572 is output to the D/A converter 55 as is.
したがって、スイッチ５７５をオフにしておくと、補償
指令値Ｃ３，は完全に重力バランスをとるのに必要な値
より小さくなり、スイッチ５７５をオンにしておくと、
補償指令値Ｃ８，は完全に重力バランスをとるのに必要
な値より大きくなる。Therefore, if the switch 575 is turned off, the compensation command value C3, will be smaller than the value required to achieve perfect gravity balance, and if the switch 575 is turned on, the compensation command value C3,
The compensation command value C8 is larger than the value required to achieve perfect gravity balance.
このスイッチ５７５のオン・オフはロボット１の作業姿
勢により選択するが、第２図に示した例のように工具（
この例ではナツトランナ１４）を下に向けた作業姿勢の
場合は、通常このスイッチ５７５をオフにしておく。The on/off status of this switch 575 is selected depending on the working posture of the robot 1, but as in the example shown in FIG.
In this example, when the nut runner 14) is in the working position facing downward, this switch 575 is normally turned off.
なお、このデータセレクタ５７４は、中央処理部２３か
らの作業完了信号Ｏｓが入力する（Ｈ″になる）と、そ
れまでのセレクト状態を反転し、加算器５７２からのデ
ータを出力していた時は減算器５７３からのデータに、
減算器５７３からのデータを出力していた時は加算器５
７２からのデータに出力を切換える。When the data selector 574 receives the work completion signal Os from the central processing unit 23 (becomes H''), it inverts the previous selection state and outputs the data from the adder 572. is the data from the subtractor 573,
When the data from the subtracter 573 is being output, the adder 5
Switch the output to data from 72.
ＣＰＵ５１から出力される対軸６用の駆動モータ１１に
対する補償指令値Ｃ８ｂを入力して、補償指令値Ｃ８２
を若干アンバランスにし、第２アーム７に一定方向の付
勢力を与えるためのアンバランス付与回路５８も、上述
のアンバランス付与回路５７と全く同様に構成されてい
る。Input the compensation command value C8b for the drive motor 11 for the paired shaft 6 output from the CPU 51, and set the compensation command value C82.
The unbalance applying circuit 58 for making the second arm 7 slightly unbalanced and applying a biasing force in a certain direction to the second arm 7 is also configured in exactly the same manner as the unbalance applying circuit 57 described above.
このようにして、重力バランス補償回路５０から出力さ
れる補償指令値Ｃ８１，Ｃ８２を、力抜き制御時に第１
図の指令値制御回路４５の切換スイッチＳＷｓ ＊ Ｓ
Ｗ＋ｏを介して加算回路３２及びモータ１１用の図示し
ないサーボ制御部における同様な加算回路に入力させ、
速度制御部２日、・・・から出力される電流指令値Ｓ１
＋・・・がゼロであっても、電流制御部２７．・・・に
よって重力補償用の駆動電流をモータ４及び１１に流し
、第１．第２アーム５，７及びハンド９の自重に略抗し
得る軸トルクを発生させるので、第１．第２７−ム５゜
７が重力バランスを略保って作業姿勢を保持することが
できる。In this way, the compensation command values C81 and C82 output from the gravity balance compensation circuit 50 are adjusted to the first
Changeover switch SWs*S of the command value control circuit 45 shown in the figure
input to the adder circuit 32 and a similar adder circuit in a servo control section (not shown) for the motor 11 via W+o,
Current command value S1 output from the speed control unit 2nd...
+... is zero, the current control unit 27. ... causes a drive current for gravity compensation to flow through the motors 4 and 11, and the first... Since the shaft torque that can substantially resist the weight of the second arms 5, 7 and hand 9 is generated, the first. The 27th arm 5.7 can maintain the working posture while substantially maintaining the gravity balance.
しかし、上述のように補償指令値が完全に重力バランス
をとるのに必要な値より若干小さい値になっていると、
そのアンバランスにより第１．第２アーム５，７が自重
による回動力に完全には抗しきれずに下方へ回動しよう
とする付勢力が与えられることになるが、その効果につ
いては後述する。However, as mentioned above, if the compensation command value is slightly smaller than the value required to achieve perfect gravity balance,
Due to this imbalance, the first. The second arms 5 and 7 are not able to completely resist the rotational force due to their own weight, but a biasing force is applied to the second arms 5 and 7 so that they tend to rotate downward, and the effect thereof will be described later.
なお、この実施例では、ハンド９のナツトランナ１４は
、力抜き状態では常に垂直（鉛直）に保持され、手首軸
８に加わるハンド９の自重は常に鉛直方向に作用するよ
うにしであるためモーメントは発生しないので、手首軸
単独での重力補償は行なっていない。In this embodiment, the nut runner 14 of the hand 9 is always held vertically (vertically) in a relaxed state, and the weight of the hand 9 applied to the wrist shaft 8 is always applied in the vertical direction, so the moment is Since this does not occur, gravity compensation for the wrist axis alone is not performed.
また、この重力バランス補償回路５０の機能を第１図の
中央処理部２３に持たせて、共通のＣＰＵによって時分
割処理させるようにしてもよい。Further, the function of the gravity balance compensation circuit 50 may be provided in the central processing unit 23 of FIG. 1, and time-sharing 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. 7 to 10.
第７図に示すこの実施例における追従力補償回路６０は
、粘性補償回路６１．慣性補償回路６２゜及び動摩擦補
償回路６３と、これらの各補償回路から出力される補償
指令値のレベルを調整するたメツボリュームｖＲ１，ｖ
Ｒ２，ｖＲ３と、力センサ１３からの信号によって追従
力を検出する追従力検出回路６４と、起動力補償回路６
５．力補償回路６６と及び補償切換回路６７と、起動力
補償回路６５及び力補償回路６６から出力される補償指
令値のレベルをそれぞれ調整するためのボリュームｖＲ
４、Ｖ Ｒ５と、アンバランス付与回路６８と、各ボリ
ュームＶＲ，〜Ｖ Ｒｓ及びアンバランス付与回路６８
からの出力を加算する加算回路６日とによって構成され
ている。The following force compensation circuit 60 in this embodiment shown in FIG. 7 includes a viscosity compensation circuit 61. The inertia compensation circuit 62°, the dynamic friction compensation circuit 63, and the volume vR1,v for adjusting the level of the compensation command value output from each of these compensation circuits.
R2, vR3, a following force detection circuit 64 that detects the following force based on the signal from the force sensor 13, and a starting force compensation circuit 6.
5. A volume vR for adjusting the level of the compensation command value output from the force compensation circuit 66, the compensation switching circuit 67, the starting force compensation circuit 65, and the force compensation circuit 66, respectively.
4. VR5, unbalance imparting circuit 68, each volume VR, to V Rs and unbalance imparting circuit 68
It is composed of an adder circuit that adds the outputs from the six adder circuits.
粘性補償回路６１は、ロボット１の可動部が変位する時
にその原軸３に発生する粘性抵抗を相殺する軸トルクを
モータ１０に発生させるための補償指令値を出力する回
路で、オペアンプ○Ｐ１と入力抵抗Ｒ１，帰還抵抗Ｒ２
からなる増幅度が１より小さい増幅器である。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. Input resistance R1, feedback resistance R2
This is an amplifier with an amplification factor of less than 1.
そして、ロボット１の追従動作期間Ｔの間にタコジェネ
レータ３日によって、原軸３の回動変位の速度に応じて
第Ｓ図（イ）に示すように発生される速度フィードバッ
ク値である電圧信号Ｖｔｇを入力して、粘性補償指令値
として同図（ロ）に示すような入力に比例した電圧信号
Ｖａを出力する。A voltage signal, which is a speed feedback value, is generated by the tacho generator 3 during the follow-up operation period T of the robot 1 according to the rotational displacement speed of the master shaft 3 as shown in FIG. Vtg 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. 9(a) is input from the tacho generator 3rd, a pulse-like voltage signal Vb(as shown in FIG. 9(c)) is input as an inertia compensation command value.
It outputs a polarity that generates torque 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 VR6 for generating a comparison voltage V r 1.
そして、タコジェネレータ３日から第９図（イ）に示す
ような電圧信号Ｖｔｇが入力すると、動摩擦補償指令値
として入力速度が多少増加した時から零になるまで、同
図（ニ）に示すような矩形波の電圧信号Ｖｃ（追従方向
のトルクを発生させる極性）を出力する。When a voltage signal Vtg as shown in Fig. 9 (A) is input to the tacho generator from the 3rd day, the dynamic friction compensation command value is changed from the time when the input speed increases slightly until it becomes zero as shown in Fig. 9 (D). A rectangular wave voltage signal Vc (with a 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 tacho generator 3 is reversed, so each of these compensation circuits 61
, 62, 63 are also inverted.
追従力検出回路６４は、例えば第８図に示すように、ブ
リッジ回路６４１と直流アンプ６４２とローパスフィル
タ６４３によって構成される。For example, as shown in FIG. 8, the follow-up force detection circuit 64 includes a bridge circuit 641, a DC amplifier 642, and a low-pass filter 643.
ブリッジ回路６４１は、第４図の力センサ１３の４個の
センサ素子１３６のうち、例えば直角に配置された２個
づつのセンサ素子を直列に接続したセンサ素子群１３６
ａと１３６ｂを２辺とし、抵抗Ｒａ、Ｒｈを他の２辺と
して、ａ−ｂ間に電源Ｅによって電圧を印加されており
、ｃ −ｄ間にセンサ素子群１３６ａ、１３６ｂの抵抗
値変化に応じた電圧を出力する。The bridge circuit 641 is a sensor element group 136 in which, for example, two sensor elements arranged at right angles are connected in series among the four sensor elements 136 of the force sensor 13 in FIG.
With a and 136b as two sides and resistors Ra and Rh as the other two sides, a voltage is applied between a and b by a power source E, and a voltage is applied between c and d depending on the resistance value change of the sensor element groups 136a and 136b. Outputs the corresponding voltage.
直流アンプ６４２は、オペアンプＯＰ、と入力抵抗Ｒｅ
、Ｒｄ及び帰還ボリュームＶＲｆからなリ、ブリッジ回
路６４１が出力する電圧を直流増幅する。その増幅度は
ボリュームＶＲｆによって調整される。The DC amplifier 642 includes an operational amplifier OP and an input resistor Re.
, Rd and feedback volume VRf, the voltage output from the bridge circuit 641 is DC amplified. The degree of amplification is adjusted by the volume VRf.
ローパスフィルタ６４３は、積分回路を構成する抵抗Ｒ
ｅ 、コンデンサＣａと、バッファアンプを構成するオ
ペアンプ○Ｐ、とからなり、直流アンプ６４２によって
増幅された検出信号のノイズ成分を除去して、第１０図
（イ）に示すような追従力に応じた電圧信号■ｄを出力
する。The low-pass filter 643 includes a resistor R that constitutes an integrating circuit.
e is composed of a capacitor Ca and an operational amplifier ○P that constitutes a buffer amplifier, and removes the noise component of the detection signal amplified by the DC amplifier 642, and responds to the tracking force as shown in FIG. 10 (a). Outputs the voltage signal ■d.
この信号Ｖｄが第７図の起動補償回路６５及び力補償回
路６６に入力される。This signal Vd is input to the starting compensation circuit 65 and force compensation circuit 66 in FIG.
起動力補償回路６５は、オペアンプＯＰ４と入力抵抗Ｒ
７及び帰還抵抗Ｒ８からなる増幅度の大きい増幅器で、
入力信号Ｖｄが発生すると直ちに飽和レベルまで増幅し
て、第１０図（ロ）に示すような矩形波に近い電圧信号
Ｖｅを出力する。The starting force compensation circuit 65 includes an operational amplifier OP4 and an input resistor R.
7 and a feedback resistor R8, which has a large amplification factor.
When the input signal Vd is generated, it is immediately amplified to the saturation level and a voltage signal Ve close to a rectangular wave as shown in FIG. 10 (b) is output.
しかし、追従動作の開始後、第１図のタコジェネレータ
３Ｓからの電圧信号Ｖ ｔ ｇがある程度大きくなると
、それを入力する補償切換回路６７のリレーＲＹが作動
して、その常閉接点Ｓｙを開くため、電圧信号Ｖｅがボ
リュームＶＲ４に印加されなくなり、ボリュームｖＲ４
に印加される補償指令値としての電圧信号Ｖｆは第１０
図（ハ）に示すように起動時のみの信号となる。However, after the start of the follow-up operation, when the voltage signal V t g from the tachogenerator 3S in FIG. Therefore, the voltage signal Ve is no longer applied to the volume VR4, and the voltage signal Ve is no longer applied to the volume VR4.
The voltage signal Vf as the compensation command value applied to the 10th
As shown in Figure (c), this signal is only used at startup.
補償切換回路６７は、オペアンプＯＰ６と入力抵抗１１
．半固定の帰還抵抗Ｒ，２及び比較電圧Ｖｒ２を発生さ
せるためのボリュームＶＲ７からなるヒステリシスをも
った比較器と、その出力がハイレベルになった時に作動
するリレーＲＶとからなり。The compensation switching circuit 67 includes an operational amplifier OP6 and an input resistor 11.
．． It consists of a comparator with hysteresis consisting of a semi-fixed feedback resistor R,2 and a volume VR7 for generating a comparison voltage Vr2, and a relay RV that operates when its output becomes high level.
ｖｔｇ＞ｖｒ２ になった時にオペアンプ○Ｐ６の出力
がＬ″からＨ″になり、その後はｖｔｇが「０」になる
までその状態を保持するように半固定抵抗Ｒ１２によっ
てヒステリシス量を調整する。The amount of hysteresis is adjusted by the semi-fixed resistor R12 so that when vtg>vr2, the output of the operational amplifier ○P6 changes from L'' to H'', and thereafter maintains this state until vtg becomes "0".
なお、追従方向を反転し得る場合には、タコジ−エネレ
ータ３日からの速度フィードバック信号Ｖ ｔ ｇが負
になる場合があるので、この補償切換回路６７を入力信
号Ｖ ｔ ｇが正負いずれであってもその絶対値が所定
値を越えたら出力を反転するウィンドコンパレータとリ
レーＲｙによって構成するとよい。Note that if the following direction can be reversed, the speed feedback signal V t g from the tachometer energizer 3rd may become negative, so the compensation switching circuit 67 is connected to the compensation switching circuit 67 regardless of whether the input signal V t g is positive or negative. However, if the absolute value exceeds a predetermined value, the output may be inverted by a window comparator and a relay Ry.
力補償回路６６は、オペアンプＯＲ５と入力抵抗Ｒ９，
帰還抵抗１’１−１０からなる若干遅延特性のある増幅
器で、入力信号Ｖｔｇがある程度増加してからなくなる
まで、第１０図（ニ）に示すような入力に比例した電圧
信号Ｖｇを補償指令値として出力する。The force compensation circuit 66 includes an operational amplifier OR5 and an input resistor R9,
An amplifier with a slight delay characteristic consisting of a feedback resistor 1'1-10 is used to compensate the voltage signal Vg proportional to the input as shown in Figure 10 (d) until the input signal Vtg increases to a certain extent and disappears. Output as .
アンバランス付与回路６８は、アンバランス力設定器と
しての一対のボリュームＶＲ８、ＶＲ９と３点切換スイ
ッチＳＷからなり、ボリュームＶ Ｒｓは正電源十Ｖと
アース間に接続されて正の分圧電圧を出力し、ボリュー
ムＶＲ９は負電源−■とアース間に接続されて負の分圧
電圧を出力する。その正負の電圧又はゼロ電圧（アース
）を切換スイッチＳＷによって選択してアンバランス信
号ΔＶとして出力する。The unbalance imparting circuit 68 consists of a pair of volumes VR8 and VR9 as unbalance force setting devices and a three-point changeover switch SW. The volume VR9 is connected between the negative power supply -■ and the ground to output a negative divided voltage. The positive and negative voltages or zero voltage (earth) are selected by the changeover switch SW and outputted as an unbalanced signal ΔV.
この切換スイッチＳＷを電子切換スイッチ（アナログス
イッチ）にすれば、中央処理部２３がらの信号によって
作業完了時等にその切換状態を制御することができる。If this changeover switch SW is an electronic changeover switch (analog switch), the switching state can be controlled by a signal from the central processing section 23 when the work is completed, etc.
上記の各補償回路６１，６２，６３．Ｅｉ５゜−３１＝
６６の各出力信号Ｖａｙ Ｖｂ、Ｖｃ、Ｖｆ、Ｖｇは、
それぞれボリュームＶ Ｒ１〜■Ｒ５によってレベル調
整された後、アンバランス付与回路６８からのアンバラ
ンス信号Δ■と共に、加算回路６日によってアナログ的
に加算されて追従補償指令値Ｃ８３として出力される。Each of the above compensation circuits 61, 62, 63. Ei5°-31=66 output signals Vay Vb, Vc, Vf, Vg are as follows:
After their levels are adjusted by the respective volumes VR1 to R5, they are added together with the unbalance signal Δ■ from the unbalance imparting circuit 68 in an analog manner by an adder circuit 6, and are output as a follow-up compensation command value C83.
加算回路６９は、オペアンプＯＰ７と５個のボリューム
ＶＲ，〜ｖＲ５及びアンバランス付与回路６８からの各
信号を入力する６個の入力抵抗Ｒ，３〜Ｒ１８と帰還抵
抗ＲＩ９とからなる非反転増幅器である。The adder circuit 69 is a non-inverting amplifier consisting of an operational amplifier OP7, five volumes VR, to vR5, and six input resistors R, 3 to R18, which receive signals from the unbalance imparting circuit 68, and a feedback resistor RI9. be.
この加算回路６日から出力される追従補償指令値（電圧
信号）Ｃ８３を、第１図の切換スイッチＳＷ、、を介し
て加算回路４２へ入力させることにより、速度制御部３
日から出力される電流指令値Ｓ２がゼロであっても、電
流制御部３７によってこの追従補償指令値Ｃ８３に応じ
た駆動電流をモータ１０に流し、追従動作時に原軸３に
発生する粘性抵抗力、慣性力、静摩擦力及び動摩擦力等
に対抗する追従力を軽減する軸トルクを発生させ、略無
抵抗で外力に追従して回動できるようにする。By inputting the follow-up compensation command value (voltage signal) C83 output from the adder circuit 6 to the adder circuit 42 via the changeover switch SW shown in FIG.
Even if the current command value S2 outputted from the motor is zero, the current control section 37 causes a drive current corresponding to the tracking compensation command value C83 to flow through the motor 10, and a viscous resistance force is generated in the original shaft 3 during the tracking operation. , generates an axial torque that reduces the following force that opposes inertial force, static friction force, dynamic friction force, etc., and enables rotation following external force with almost no resistance.
なお、アンバランス付与回路６８の切換スイッチＳＷを
接点ａ又はｂに切換えることにより、原軸の追従力補正
を過大又は過小にして、追従方向又はその逆方向に付勢
力を与えることができる。By switching the changeover switch SW of the unbalance imparting circuit 68 to contact a or b, it is possible to make the original axis follow-up force correction too large or too small, and to apply an urging force in the follow-up direction or the opposite direction.
ここで、この追従力補償回路６０の調整方法について説
明する。Here, a method for adjusting the following force compensation circuit 60 will be explained.
先ず、第１図の切換回路４６の切換スイッチ４８を接点
り側（図示と反対）に、切換スイッチ４７を接点ｄ側（
図示の状態）に切換えて、ロボット１を力抜き状態にし
たうえで、次の手順で調整する。First, the changeover switch 48 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, make adjustments according to the following procedure.
なお、この時にボリュームｖＲ４，ｖＲ５の出力を「０
」にし、アンバランス付与回路６８の切換スイッチＳＷ
を第７図に示すように接点Ｃ（アース側）に切換えてお
く。At this time, the output of volumes vR4 and vR5 is set to "0".
", and set the changeover switch SW of the unbalance imparting circuit 68.
is switched to contact C (earth side) as shown in Fig. 7.
（ａ）補償したい軸（この実施例では原軸３）を一定の
低速で動かし、第７図のボリュームＶＲ３。(a) Move the axis to be compensated (original axis 3 in this example) at a constant low speed, and set the volume VR3 in FIG.
Ｖ Ｒ６を調整して、追従力ができるだけ軽くなるよう
に動摩擦補償を調整する。Adjust VR6 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.
（ｄ）第２図のナツトランナ１４を外力によって追従移
動させた時の動き始めの追従力が最も小さくなり、しか
も自分で動き出すことがないように、ボリュームＶ Ｒ
４によって起動力補償を調整する。(d) The volume V R is adjusted so that when the nut runner 14 shown in Fig. 2 is moved by an external force, the following force at the beginning of the movement is the smallest and it does not start moving on its own.
4 to adjust the starting force compensation.
（ｅ）ナツトランナ１４を外力によって追従動作ささた
時の起動後の略一定速度での追従力が最も小さくなり、
しかも自分で動き出すことがないように、ボリュームＶ
Ｒｓによって力補償を調整する。(e) When the nut runner 14 is subjected to a following operation by an external force, the following force becomes the smallest at a substantially constant speed after startup,
Moreover, to prevent it from starting to move on its own, the volume V
Adjust force compensation by Rs.
（ｆ）ナツトランナ１４をボルト１日に係合させた時、
その追従方向に応じてアンバランス付与回路６日の切換
スイッチＳＷをａ又はｂに切換えて、追従方向に若干の
押付力を発生させ、コンベア１７（第２図）の速度変動
や振動等によって容易にその係合が外れることがない程
度にボリュームＶ Ｒａ又はｖＲ９によってアンバラン
ス力（付勢力）の調整を行なう。(f) When the nut runner 14 is engaged on the bolt 1,
Depending on the following direction, the changeover switch SW of the unbalance imparting circuit 6 is switched to a or b to generate a slight pressing force in the following direction, so that it is easily affected by speed fluctuations, vibrations, etc. of the conveyor 17 (Fig. 2). The unbalanced force (biasing force) is adjusted by the volume VRa or vR9 to such an extent that the engagement will not be disengaged.
次に、第１図における速度制限回路７０の具体例につい
て、第１１図及び第１２図によって説明する。Next, a specific example of the speed limiting circuit 70 shown in FIG. 1 will be explained with reference to FIGS. 11 and 12.
この速度制限回路７０は、制限速度設定器７１とウィン
ドコンパレータ７２と減速指令回路７３と２個の電子切
換スイッチ（アナログスイッチ）７４．７５によって構
成されている。The speed limiter circuit 70 includes a speed limiter 71, a window comparator 72, a deceleration command circuit 73, and two electronic changeover switches (analog switches) 74 and 75.
制限速度設定器７１は、正電源十Ｖとアース間に両端を
接続したボリュームＶ Ｒａと、負電源−■とアース間
に両端を接続したボリュームＶＲｂからなり、モータ４
の回転方向に応じた極性でその回転速度比例した電圧信
号として、第１図のタコジェネレータ２９から発生する
第１２図（イ）に示すような速度フィードバック信号Ｖ
ｔｇの正の許容限界に相当する電圧Ｖａと負の許容限界
に相当する電圧ｖｂを設定して、ウィンドコンバレー＝
３５−
タフ２へ供給する。The limit speed setting device 71 consists of a volume VRa whose both ends are connected between the positive power supply 1 V and the ground, and a volume VRb whose both ends are connected between the negative power supply -■ and the ground.
A speed feedback signal V as shown in FIG. 12 (A) generated from the tacho generator 29 in FIG.
By setting the voltage Va corresponding to the positive allowable limit of tg and the voltage vb corresponding to the negative allowable limit, the window convergence =
35- Supply to Tough 2.
ウィンドコンパレータ７２は、抵抗Ｒｆ−Ｒｋと比較器
としてのオペアンプＯＰａ、ＯＰｂとダイオードＤａ、
Ｄｂからなり、タコジェネレータ２日からの速度フィー
ドバック信号Ｖｔｇを抵抗Ｒｆ、Ｒｉを介して、それぞ
れオヘアンプＯＰａの非反転入力端子とオペアンプＯＰ
ｂの反転入力端子に入力し、制限速度設定器７１による
設定電圧Ｖａ、Ｖｂを、それぞれ抵抗Ｒｇ、Ｒｈを介し
てオペアンプ○Ｐａの反転入力端子とオペアンプ○ｐｂ
の非反転入力端子に入力する。The window comparator 72 includes a resistor Rf-Rk, operational amplifiers OPa and OPb as comparators, and a diode Da.
The speed feedback signal Vtg from the tacho generator 2 is connected to the non-inverting input terminal of the operational amplifier OPa and the operational amplifier OP through resistors Rf and Ri, respectively.
The set voltages Va and Vb from the speed limit setter 71 are input to the inverting input terminal of operational amplifier ○Pa and the operational amplifier ○pb through resistors Rg and Rh, respectively.
input to the non-inverting input terminal of
したがって、ｖｂ≦Ｖｔｇ≦Ｖａ であれば、オペア
ンプ○Ｐａ、ＯＰｂの出力はいずれもＬ″であるが、Ｖ
ｔｇ＞Ｖａ又はＶｔ、ｇ＜Ｖｂ になルト、オペアン
プＯＰａ、○Ｐｂのいずれかの出力がＨ″になり、それ
をダイオードＤａ、Ｄｂによって加算したウィンドコン
バレ〜り７２の出力Ｃｍは、第１２図（ロ）に示すよう
になる。Therefore, if vb≦Vtg≦Va, the outputs of operational amplifiers ○Pa and OPb are both L'', but V
When tg>Va or Vt, g<Vb, the output of either the operational amplifier OPa or ○Pb becomes H'', and the output Cm of the wind converter circuit 72, which is added by the diodes Da and Db, is The result is as shown in Figure 12 (b).
一方、減速指令回路７３は、オペアンプＯＰ ｃと抵抗
Ｒｍ −Ｒｏ及び出力調整用のボリュームＶ Ｒｃから
なる増幅度の大きい増幅器であり、速度フィードバック
信号ｖＬｇに対して第１０図（ハ）に示すような電圧信
号＋Ｖｃ又は−Ｖｃ（その大きさはボリュームｖＲｃに
よって調整できる）を減速指令信号Ｓｃとして出力する
。On the other hand, the deceleration command circuit 73 is an amplifier with a large amplification degree consisting of an operational amplifier OPc, a resistor Rm-Ro, and a volume VRc for output adjustment. A voltage signal +Vc or -Vc (the magnitude of which can be adjusted by a volume vRc) is output as a deceleration command signal Sc.
そして、アナログスイッチ７４．７５の入力端子Ａはい
ずれもアースに接続し、アナログスイッチ７４の入力端
子Ｂに減速指令回路７３からの減速指令信号ＳＣを、ア
ナログスイッチ７５の入力端子Ｂには速度フィードバッ
ク信号Ｖｔｇをそれぞれ入力する。The input terminals A of the analog switches 74 and 75 are both connected to ground, the deceleration command signal SC from the deceleration command circuit 73 is input to the input terminal B of the analog switch 74, and the speed feedback signal is input to the input terminal B of the analog switch 75. A signal Vtg is input to each.
また、各アナログスイッチ７４．７５の切換信号端子に
ウィンドコンパレータ７２の出力信号Ｃｍを入力し、こ
の信号ＣｍがＬ″の時はアナログスイッチ７４．７５が
いずれも入力端子Ａのアース電位を出力し、信号Ｃｍが
Ｈ″の時は、アナログスイッチ７４．７５がそれぞれ入
力端子Ｂの減速指令信号Ｓｃと速度フィードバック信号
ｖＬｇを出力する。In addition, the output signal Cm of the window comparator 72 is input to the switching signal terminal of each analog switch 74, 75, and when this signal Cm is L'', the analog switches 74, 75 output the ground potential of the input terminal A. , when the signal Cm is H'', the analog switches 74 and 75 output the deceleration command signal Sc and the speed feedback signal vLg of the input terminal B, respectively.
したがって、この速度制限回路７０から出力されて第１
の切換スイッチＳＷｌ、ＳＷ２の各接点ａに供給される
信号Ｇ １ ＋ Ｇ ２は、それぞれ第１２図（ニ）、
（ホ）に示すようになる。Therefore, the speed limit circuit 70 outputs the first
The signals G 1 + G 2 supplied to the contacts a of the changeover switches SWl and SW2 are shown in FIG. 12 (d), respectively.
It becomes as shown in (e).
そこで、第１図の切換スイッチＳＷ ｓ ＋ Ｓ Ｗ
２が図示にように接点ａ側に切換わると、当初は速度制
限回路７０からの信号Ｇ １ ＋ ０２がいずれも「０
」 （アース電位）になっているので、速度制御部２６
の２つの入力値に偏差がなくなり、その偏差に基づく電
流指令値Ｓ１はゼロになる。Therefore, the changeover switch SW s + SW in Fig. 1
2 is switched to the contact a side as shown in the figure, the signal G 1 + 02 from the speed limiter circuit 70 is initially "0".
” (earth potential), the speed control unit 26
There is no deviation between the two input values, and the current command value S1 based on the deviation becomes zero.
そのため、電流制御部２７は加算回路６２を介して入力
する重力バランス補償回路５０からの補正指令値Ｃ８１
に応じた駆動電流をモータ４に流し、その発生トルクに
よって第１アーム５から先の重力によるモーメントに対
抗させてバランスを ゛保つ。Therefore, the current control unit 27 receives the correction command value C81 from the gravity balance compensation circuit 50 input via the addition circuit 62.
A driving current corresponding to the amount of force is applied to the motor 4, and the generated torque counteracts the moment due to gravity from the first arm 5 forward, thereby maintaining balance.
この場合、発生するトルクと第１アームから先の重力に
よるモーメントとが完全にバランスしていればモータ４
は回転しないが、前述のように補正指令値Ｃ８，をアン
バランスにしておくと、補正が過小であれば重力の作用
方向（下方）に、過大であればその反対方向（上方）へ
第１アーム５を移動させる付勢力が作用し、モータ４が
回転する。In this case, if the generated torque and the moment due to gravity from the first arm are perfectly balanced, the motor 4
does not rotate, but if the correction command value C8 is made unbalanced as described above, if the correction is too small, it will move in the direction of gravity (downward), and if it is too large, it will move in the opposite direction (upward). A biasing force is applied to move the arm 5, and the motor 4 rotates.
そして、そのモータ４の回転速度が、第１１図の制限速
度設定器７１によって設定された制限速度以内であれば
、ｖｂ≦Ｖ ｔ ｇ≦Ｖａの条件を満しているのでその
まま回転回転し続けるが、制限速度を超えるとＶｔｇ＞
Ｖａ又はＶｔｇ＜Ｖｂになるため、第１１図のウィンド
コンパレータ７２の出力信号ＣｍがＨ″になり、アナロ
グスイッチ７４．７５が減速指令信号Ｓｃ及び速度フィ
ードバック信号Ｖ ｔ ｇをそれぞれ速度制限回路７０
の出力信号Ｇ１．Ｇ２として出力するようになる。If the rotational speed of the motor 4 is within the speed limit set by the speed limit setting device 71 shown in FIG. However, when the speed limit is exceeded, Vtg>
Since Va or Vtg<Vb, the output signal Cm of the window comparator 72 in FIG.
The output signal G1. It will be output as G2.
速度制御部２６はそれを入力し、その２つの信号の偏差
に応じてその偏差をなくすように電流指令値Ｓ１を出力
する。The speed control unit 26 inputs this and outputs a current command value S1 according to the deviation between the two signals so as to eliminate the deviation.
この電流指令値Ｓ１と補正指令値Ｃ８１とが加算回路３
２によって加算されて（この場合Ｓ１とＣ８１は極性が
反対になっているので絶対値は小さくなる）電流制御部
２７に入力し、モータ４に流す駆動電流を減少させる。This current command value S1 and correction command value C81 are added to the adder circuit 3.
2 (in this case, the polarities of S1 and C81 are opposite, so the absolute value is small) and input to the current control section 27 to reduce the drive current flowing to the motor 4.
すなわち、制限速度になるまで速度フィードバラフカ生
きてモータ４の回転速度すなわち第１７−ム５の回動速
度を減速し続ける。そして、制限速度以内になると前述
の重力バランス補償のみの状態に戻る。That is, the speed feed balance continues to reduce the rotational speed of the motor 4, that is, the rotational speed of the 17th arm 5, until the speed limit is reached. Then, when the speed falls within the speed limit, the state returns to only the gravity balance compensation described above.
なお、このような速度制限回路は、第２アーム駆動用の
モータ１１のサーボ制御部には勿論であるが、追従力補
償もアンバランスにする場合には原軸駆動用のモータ１
０のサーボ制御部にも設けることが望ましい。Note that such a speed limiting circuit is of course applied to the servo control section of the motor 11 for driving the second arm, but if the following force compensation is also to be unbalanced, it is necessary to install the speed limiting circuit in the motor 1 for driving the original shaft.
It is desirable to provide this also in the servo control section of 0.
次に、前述のようにプレイバック制御と力抜き制御の切
換えが可能な第１図の制御装置を用いて、第２図のロボ
ット１に、コンベア１７に載って一定の速度で移動する
ワーク１８のボルト１９を締め付ける作業を行なわせる
場合の動作について、第１３図のフローチャートも参照
しながら説明する。Next, using the control device shown in FIG. 1 which is capable of switching between playback control and stress relief control as described above, the workpiece 18 mounted on the conveyor 17 and moving at a constant speed is controlled by the robot 1 shown in FIG. The operation for tightening the bolt 19 will be explained with reference to the flowchart in FIG. 13.
なお、ロボット１に以下に述べる締め付は作業を行なわ
せるために必要なティーチング作業は予めしであるもの
とする。It is assumed that the teaching work required to have the robot 1 perform the tightening work described below has been done in advance.
また、以下の説明では、第１図の切換回路４６における
切換スイッチ４８は接点ｇ側に切換えであるものとし、
その状態で常時は中央処理部２３からの制御信号Ｏｓが
Ｌ″になっているため、第１図のトランジスタ４日はオ
フで指令値制御回路４５のリレーコイルＬ１への通電を
断っているので、各切換スイッチ５ＷＩ−８Ｗ８．の可
動接片Ｃは全て固定接点す側に切換わっており、原軸６
゜第１．第２アーム５，７．ハンド９を夫々回動させる
各モータ４，１０．１１等のプレイバック制御が可能で
ある。In addition, in the following explanation, it is assumed that the changeover switch 48 in the changeover circuit 46 of FIG. 1 is switched to the contact g side,
In this state, the control signal Os from the central processing unit 23 is always at L'', so the transistor 4 in FIG. 1 is off and the relay coil L1 of the command value control circuit 45 is cut off. , the movable contacts C of each changeover switch 5WI-8W8. are all switched to the fixed contact side, and the original axis 6
゜First. Second arm 5, 7. Playback control of each motor 4, 10, 11, etc. that rotates the hand 9 is possible.
そこで、第１３図のステップ１で先ずモータ４゜１０等
をプレイバック制御して、原軸３．第１゜第２アーム５
，７等を電位Ｍ（待避位置であればどこでもよい）へ移
動させる。Therefore, in step 1 of FIG. 13, first, playback control is performed on the motors 4 and 10, and the original shaft 3. 1st ° 2nd arm 5
, 7, etc., to potential M (anywhere as long as it is a retracted position).
次に、ハンド９に取り付けたナツトランナ１４のソケッ
ト１４ａが、コンベア１７によって搬送されるワーク１
８上のボルト１日の移動軌跡上の予め定めた待機位置で
ある作業ポイントに位置するように、ステップ２で再び
モータ４，１０等をプレイバック制御して、第２図に示
すような所定の待機姿勢にする。Next, the socket 14a of the nut runner 14 attached to the hand 9 is connected to 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 on the workpiece 18 for one day, and when the bolt passage detector 20 detects the passage of the poles 1-19, the detection signal is as shown in FIG. The input is input to the central processing unit 23, and the determination in step 3 becomes YES due to the fidgeting, and in step 4, playback control is performed again to vertically lower the nut runner 14.
そして、ナツトランナ１４のソケット１４．がボルト１
９を衝えた時点、すなわち位置決め完了時点でステップ
５からステップ６へ進み、制御信号Ｏ８をＨ″にしてト
ランジスタ４日をオンにする。Then, the socket 14 of the nut runner 14. is bolt 1
9, that is, when the positioning is completed, the process proceeds from step 5 to step 6, where the control signal O8 is set to H'' and the transistor 4 is turned on.
それによって、指令値制御回路４５のリレーコイルＬ１
に３Ｉｉ電され、その各切換スイッチＳＷ。Thereby, the relay coil L1 of the command value control circuit 45
3Ii power is applied to each selector switch SW.
〜Ｓ Ｗ ＋ ＋の可動接片Ｃが全て第１図に示すよう
に固定接点ａ側に切換わるので、各軸の駆動用モータ４
，１０．１１等は全てフリーになり、原軸３及び第１．
第２アーム５，７等の可動部は外力を受ければ自由に動
く力抜き状態となるが、重力バランス補償回路５０から
の補償指令値Ｃ８，及びＣ８２によって、モータ４，１
１には自重によるモーメントに抗する軸トルクを発生す
るための駆動電流が流されるため、バランスが保たれて
第１゜第２アーム５，７の姿勢が崩れることはない。~S W + + All movable contact pieces C are switched to the fixed contact a side as shown in Fig. 1, so the drive motor 4 of each axis
, 10, 11, etc. are all free, and the original axis 3 and the 1st .
When the movable parts such as the second arms 5 and 7 receive an external force, they are in a relaxed state where they can move freely, but the compensation command values C8 and C82 from the gravity balance compensation circuit 50 cause the motors 4 and 1
Since a drive current is applied to the arm 1 to generate an axial torque that resists the moment due to its own weight, the balance is maintained and the postures of the 1° and 2nd arms 5 and 7 do not collapse.
この時、補償指令値ｃｓ、、ｃｓ２を若干少なめにアン
バランスにしておくと、第１アーム５から先が完全には
重力に抗しきれず、重力の作用方向に若干付勢され、ナ
ツトランナ１４のソケット１４、をボルト１日に押付け
るため、両者の係合が確実になり、振動等によって外れ
る恐れがなくなる。At this time, if the compensation command values cs, , cs2 are slightly unbalanced, the part from the first arm 5 onward cannot fully resist gravity, and is slightly biased in the direction of gravity, causing the nut runner 14 to Since the socket 14 is pressed against the bolt 1, engagement between the two is ensured, and there is no possibility of the bolt coming off due to vibration or the like.
次に、ステップ７でナツトランナ１４を駆動してソケッ
ト１４ａを回転させ、ボルト１日を締め付ける。Next, in step 7, the nut runner 14 is driven to rotate the socket 14a and tighten the bolt.
このように、ナツトランナ１４のソケット１４ａがボル
ト１日を衝え込んだ状態で締め付は作業を行なっている
間も、コンベア１７はワーク１８を移動させているが、
ロボット１の原軸３及び第１、第２アーム５，７等の各
可動部は外力によって自由に動かせる状態にあるので、
ボルト１日を締め付けながらその移動に追従してハンド
Ｓは水平方向に動くことができる。In this way, while the socket 14a of the nut runner 14 is tightening the bolt with the bolt pushed in, the conveyor 17 is moving the workpiece 18.
Since each movable part of the robot 1, such as the original axis 3 and the first and second arms 5 and 7, can be moved freely by external force,
While tightening the bolt, the hand S can move horizontally to follow the movement.
その際、原軸３に作用する粘性、慣性、及び動摩擦によ
る各抵抗力は、前述の追従力補償回路６０からの粘性補
償、慣性補償、及び動摩擦補償の各指令値の和に応じて
モータ１０に駆動電流が流されるため略相殺されれるが
、それでも補償しきれない静摩力やその他の抵抗力によ
って、実際に追従動作が開始されると力センサ１３と追
従力検出回路６４による追従力検出手段から検出信号Ｖ
ｄが現われ、その大きさに応じて起動力補償とその後の
力補償がなされるため、ナツトランナ１４は殆んど無抵
抗で追従移動する。At this time, each resistance force due to viscosity, inertia, and dynamic friction acting on the original shaft 3 is applied to the motor 10 according to the sum of the command values of viscosity compensation, inertia compensation, and dynamic friction compensation from the following force compensation circuit 60 described above. However, due to static friction and other resistance forces that cannot be compensated for, when the following movement actually starts, the following force is detected by the force sensor 13 and the following force detection circuit 64. Detection signal V from the means
d appears, and starting force compensation and subsequent force compensation are performed according to its magnitude, so the nut runner 14 follows the movement with almost no resistance.
その際、第７図のアンバランス付与回路６８の切換スイ
ッチＳＷをａ又はｂ側に切換えて、ボリュームｖＲ８又
はＶ Ｒ９によって必要な追従力より補償追従力の方が
若干大きくなるようにアンバランスの設定をしておけば
、ナツトランナ１４をボルト１日に対して追従方向に若
干押付ける力が発生するため、追従動作中の両者の係合
がより確実に保たれる。At that time, changeover switch SW of the unbalance imparting circuit 68 in FIG. If this setting is made, a force is generated that slightly presses the nut runner 14 against the bolt in the following direction, so that the engagement between the two during the following operation can be maintained more reliably.
そして、締め付けを開始してからの時間又は締付トルク
を計測することによって、ステップ８でボルト１Ｓの締
め付けが終了したか否かを判定し、締め付けが終了した
らステップ９でナツトランナ１４の駆動を停止し、ステ
ップ１０で作業完了信号を重力バランス補償回路に出力
して、アンバランス方向（付勢力の方向）を反転させる
。Then, by measuring the time since the start of tightening or the tightening torque, it is determined in step 8 whether or not the tightening of the bolt 1S is finished, and when the tightening is finished, the drive of the nut runner 14 is stopped in step 9. Then, in step 10, a work completion signal is output to the gravity balance compensation circuit to reverse the unbalance direction (direction of biasing force).
それによって、重力補償指令値が重力バランスを保つの
に必要な値より大きくなり、ナツトランナ１４を上方へ
移動させてボルト１日から離脱させる。As a result, the gravity compensation command value becomes larger than the value required to maintain gravity balance, and the nut runner 14 is moved upward and removed from the bolt.
この時、第１アームあるいはその他の可動部の移動速度
が制限速度を超えると、前述の速度制限回路７０等の作
用により、速度フィードバックによる減速制御が行なわ
れるので作動が安定し、特にナツトランナ１４がボルト
１日から離脱した途端に急速に上昇するようなことがな
い。At this time, if the moving speed of the first arm or other movable parts exceeds the speed limit, deceleration control is performed by speed feedback due to the action of the speed limiting circuit 70, etc. described above, so that the operation is stabilized, and the nut runner 14 in particular There is no rapid rise as soon as you leave Bolt for one day.
ステップ１１でナツトランナの離脱を判定すると、（所
定時間の経過を判定してもよい）ステップ１２で制御信
号ＯｓをＬ″にして指令値制御回路４５のレリーコイル
Ｌ１を非励磁にし、各切換スイッチＳＷ、〜ＳＷｕが全
て接点す側に切換わるので、各軸の駆動用モータ４，１
０．１１等は全てプレイバック制御が可能な状態に戻る
。When it is determined in step 11 that the nut runner is detached, in step 12 (it may be determined that a predetermined time has elapsed), the control signal Os is set to L'', the relay coil L1 of the command value control circuit 45 is de-energized, and each changeover switch SW , ~ SWu all switch to the contact side, so the drive motors 4 and 1 of each axis
0.11 etc. all return to a state where 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 work point before it moved, but the process returns from step 12 to step 1 and moves to the initial original position again, so the original axis 3 and the first
．． 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.
しかも、追従作業中にはナツトランナ１４をボルト１日
に押付ける方向の付勢力を与えてその係合を確実に保ち
、作業完了後はその付勢力を反転させることにより、設
定限度内の速度でナツトランナ１４をボルト１日から離
脱させることができる。Moreover, during the follow-up work, a biasing force is applied in the direction of pushing the nut runner 14 against the bolt 1 to ensure the engagement, and after the work is completed, the biasing force is reversed to maintain the speed within the set limit. Natstranner 14 can be removed from Bolt 1 day.
このようにして、ナツトランナをボルトから離脱させる
のは、ボルト締めは常に作業完了時間が一定でないため
、作業完了位置がまちまちになるため、プレイバックに
よって離脱させようとすると、作業完了位置によっては
締めたボルトに思わぬ負荷がかかつてしまうことがある
からで、まず、アンバランス力によって離脱させた後、
プレイバックによって作業点に戻せば、そのような問題
が解消する。The reason why the nut runner is released from the bolt in this way is that bolt tightening does not always have a constant work completion time, so the work completion position varies. This is because unexpected loads may be applied to the bolts, so first remove them due to unbalanced force, and then
Returning to the working point by playback solves such problems.
ところで、上前の実施例では押付力を重力の作用方向と
追従方向に発生させるようにしたが、例えばロボット１
の上方に設置されたコンベアによって搬送されるワーク
に対して、下側から工具を上向きにしてワークに係合さ
せて追従作業を行なわせるような場合には、重力の作用
方向と反対方向（上向き）に押付力が発生するように、
補償指令値ｃｓ、、Ｃ８２を大きい方にアンバランスに
するとよい。By the way, in the previous embodiment, the pressing force was generated in the direction of action of gravity and the following direction, but for example, when the robot 1
When a tool is to be conveyed by a conveyor installed above and the tool is engaged with the workpiece from below upwards to perform follow-up work, it is necessary to ) so that a pressing force is generated at
It is preferable to make the compensation command values cs, . . . C82 unbalanced to a larger value.
また、ロボット１が工具を横向きにして追従方向と反対
方向（例えば第２図の矢示Ｅと反対の方向）からワーク
に係合させて追従作業を行なわせるような場合には、追
従方向と逆の方向に押付力が発生するように、補償指令
値Ｃ８３を小さい方にアンバランスにすればよい。In addition, when the robot 1 engages the workpiece in the direction opposite to the following direction (for example, the direction opposite to arrow E in Fig. 2) with the tool facing sideways to perform the following work, the following direction The compensation command value C83 may be unbalanced to a smaller value so that a pressing force is generated in the opposite direction.
いずれの場合にも、作業終了後そのアンバランス方向を
反転することにより、工具をワークから離脱させること
ができる。In either case, the tool can be removed from the workpiece by reversing the unbalanced direction after the work is completed.
また、コンベア１７を止めて、静止しているワ−り１８
上のボルト１日の締め付は作業を行なう場合は、切換回
路４６における切換スイッチ４８を接点り側に切換える
と共に、切換スイッチ４７を接点ｅ側に切換えて、ボル
ト１９の静止位置と前述の作業ポイントとを一致させて
おけば、第７図のステップ３のボルト通過の判断を省略
することによって、その静止したボルト１９の締め付は
作業を行なうことができる。Also, the conveyor 17 is stopped and the stationary workpiece 18 is
When tightening the upper bolt for one 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 19 in its rest position and perform the above-mentioned work. If the points match, the bolt 19 can be tightened while it is stationary by omitting the determination of whether the bolt passes in step 3 in FIG.
さらに、切換スイッチ４８を接点り側に切換えてあれば
、切換スイッチ４７の方を必要に応じて接点ｄ側に切換
えることによって、何時でも第１゜第２アーム５，７等
を外力によって自由に動かし得る力抜き状態にすること
ができる。Furthermore, if the changeover switch 48 is switched to the contact side, the first and second arms 5, 7, etc. can be freely controlled by external force at any time 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.
なお、上記実施例では、実際の速度指令値と速度フィー
ドバック値を共に位置決め制御とは無関係な零値に切り
換えることによって電流指令値を零にするようにした例
について述べたが、この他に両値を共に位置決め制御と
は無関係な互いに等しい所定値に切換えることによって
も電流指令値を零にすることができる。あるいは、電流
指令値を直接零値に切換えるようにしてもよい。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
Now, the angle θ1. in FIG. 3 from the potentiometer 30.40. The gravity compensation value was calculated according to the data corresponding to θ2, but each angle θ1
．． Even if the optimal gravity compensation value for each vertical joint axis according to θ2 is stored as a table in the memory 52 in FIG. 5, the CPU 51 reads the gravity compensation value from the table in accordance with the input angle data. good.
さらに、追従力補償をその効果が最も大きい原軸に対し
てのみ行なうようにしたが、肩軸及び対軸等信の各軸に
対しても行なうようにすれば一層完全な補償が可能にな
る。In addition, although the following force compensation is performed only on the original axis, where the effect is greatest, even more complete compensation can be achieved if it is performed on each axis such as the shoulder axis and the opposite axis. .
ところで、この発明は第２図に示した垂直多関節型ロボ
ットに限らず、水平多関節型ロボットや平行リンクアー
ムを有する垂直多関節型ロボット等各種のロボットに適
用できる。ワーク又は工具も種々のものに適用できる。By the way, 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 parallel link arms. It can also be applied to various workpieces or tools.
なお、水平多関節型ロボットに適用する場合には重力バ
ランス補償は不要である。Note that gravity balance compensation is not required when applied to a horizontal articulated robot.
以上説明してきたように、この発明によれば、速度指令
値とロボットの可動部の速度検出系からの速度フィード
バック値との偏差に基づく指令値に応じて、ロボットの
可能部を駆動するモータの駆動電流を制御するようにし
たロボットの制御装置において、速度指令値と速度フィ
ードバック値との偏差に基づく指令値を実際の速度指令
値と速度フィードバック値に拘らず零にすることにより
ロボットの可動部を外力により自由に動かし得るように
したので、指令値制御手段として接点容量が小さく小型
で安価な電磁リレー等を用いることができ、それによっ
て接点のメンテナンス回数を少なくできるばかりか、接
点の切換時に突入電流がモータに流れることがないため
その防止対策を施さなくて済む。As described above, according to the present invention, the motor that drives the movable part of the robot 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 movable part of the robot. In a robot control device that controls drive current, a command value based on the deviation between a speed command value and a speed feedback value is set to zero regardless of the actual speed command value and speed feedback value. Since it can be moved freely by external force, it is possible to use a compact and inexpensive electromagnetic relay with small contact capacity as a command value control means, which not only reduces the number of times of contact maintenance, but also reduces the number of times when switching contacts. Since inrush current does not flow to the motor, there is no need to take measures to prevent it.
また、この方抜き制御時に可動部に一定方向の付勢力を
与えてロボット側とワーク上の係合状態を確実にすると
共に、その付勢力によって可動部〜５１−
が移動する時の速度を制限範囲内でスムーズに行なわせ
ることができ、特にロボットによる追従作業完了時に、
ハンドの退避動作もスムーズに行なわせることができる
。In addition, during this direction extraction control, a biasing force is applied to the movable part in a certain direction to ensure the engagement state between the robot side and the workpiece, and the biasing force limits the speed at which the movable part ~51- moves within a range. This can be done smoothly within the robot, especially when the robot completes the follow-up work.
The retracting motion of the hand can also be performed smoothly.
第１図はこの発明の一実施例を示す制御装置のブロック
構成図、
第２図はこの発明を適用する垂直多関節型ロボットの構
成及びその作業の説明に供するロボットまわりの外観図
、
第３図は同じくそのロボットの垂直関節軸に加わる重力
のモーメントを説明するための模式第４図は第２図にお
ける力センサ１３の構造を示す拡大斜視図、
第５図は第１図における重力バランス補償回路５０の具
体例を示すブロック回路図、
第６図は第５図におけるｃｐｕ５１の動作例を示すフロ
ー図、
第７図は第１図における追従力補償回路６０の具５２一
体側を示す回路図、
第８図は第７図における追従力検出回路６４の構成例を
示す＠Ｍ図、
第９図及び第１０図は追従力補償回路６０の作用説明の
ための各部の信号波形図、
第１１図は第１図の速度制限回路７０の具体例を示す回
路図、
第１２図は速度制限回路７０の作用説明のための各部の
信号波形図、
第１３図は第１図における中央処理装置２３の動作例を
示すフロー図である。
１・・・垂直多関節型ロボット ３・・・原軸４．１
０．１１・・・ＤＣサーボモータ５・・・第１アーム
６・・・対軸 ７・・・第２７−ム８・・・手首軸
９・・・ハンド１３・・・力センサ １４・
・・ナツトランナ１７・・・コンティニュアスコンベア
１８・・・ワーク１９・・・ボルト ２０・・・
ボルト通過検知器２２・・・力センサ ２３・
・・中央処理部２９．３９・・・タコジエネレータ
３０．４０・・・ポテンショメータ
４５・・・指令値制御回路 ４６・・・切換回路５
０・・・重力バランス補償回路
５７．５８・・・アンバランス付与回路６０・・追従力
補償回路 ６１・・・粘性補償回路６２・・・慣性補
償回路 ６３・・・動摩擦補償回路６４・・・追従
力検出回路 ６５・・・起動力補償回路６６・・・力
補償回路 ６７・・・補償切換回路６８・・・ア
ンバランス付与回路
６日・・・加算回路 ７０・・・速度制限回路
ａｍ
ト 「
第９図
追従期間
第１０図
追［期間
′１１＜Fig. 1 is a block configuration diagram of a control device showing an embodiment of the present invention; 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 invention is applied; The figure is a schematic diagram for explaining the moment of gravity applied to the vertical joint axis of the robot. Figure 4 is an enlarged perspective view showing the structure of the force sensor 13 in Figure 2, and Figure 5 is the gravity balance compensation in Figure 1. A block circuit diagram showing a specific example of the circuit 50, FIG. 6 is a flow diagram showing an example of the operation of the CPU 51 in FIG. , FIG. 8 is an @M diagram showing a configuration example of the following force detection circuit 64 in FIG. 7, FIGS. 9 and 10 are signal waveform diagrams of each part for explaining the operation of the following force compensation circuit 60, 1 is a circuit diagram showing a specific example of the speed limiting circuit 70 in FIG. 1, FIG. 12 is a signal waveform diagram of each part to explain the operation of the speed limiting circuit 70, and FIG. 13 is a diagram showing the central processing unit 23 in FIG. 1. FIG. 2 is a flow diagram showing an example of the operation. 1... Vertical articulated robot 3... Original axis 4.1
0.11...DC servo motor 5...1st arm
6... Opposite shaft 7... 27th arm 8... Wrist axis 9... Hand 13... Force sensor 14.
...Nut runner 17...Continuous conveyor 18...Work 19...Bolt 20...
Bolt passage detector 22...force sensor 23.
...Central processing unit 29.39...Tachometer generator 30.40...Potentiometer 45...Command value control circuit 46...Switching circuit 5
0...Gravity balance compensation circuit 57.58...Unbalance imparting circuit 60...Following force compensation circuit 61...Viscosity compensation circuit 62...Inertia compensation circuit 63...Dynamic friction compensation circuit 64... Following force detection circuit 65... Starting force compensation circuit 66... Force compensation circuit 67... Compensation switching circuit 68... Unbalance imparting circuit 6th... Addition circuit 70... Speed limit circuit am "Figure 9 follow-up period Figure 10 follow-up [period'11<
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. The robot control device includes: 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; and the movable part. a compensation circuit that outputs a compensation command value for generating a required torque in a motor that drives the movable part so as to apply a biasing force in a certain direction to the motor; and a command value control circuit that outputs a command value based on the deviation. and a speed limiting circuit that limits the moving speed of the movable part by controlling the drive current of the motor according to the compensation command value output by the compensation circuit when the compensation command value is set to zero. . 2. The robot control device according to claim 1, wherein the compensation circuit includes means for reversing the direction of the biasing force applied to the movable part.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP8463785A JPS61243513A (en) | 1985-04-22 | 1985-04-22 | Robot controller |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP8463785A JPS61243513A (en) | 1985-04-22 | 1985-04-22 | Robot controller |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPS61243513A true JPS61243513A (en) | 1986-10-29 |
Family
ID=13836201
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP8463785A Pending JPS61243513A (en) | 1985-04-22 | 1985-04-22 | Robot controller |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPS61243513A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022102464A1 (en) * | 2020-11-11 | 2022-05-19 | パナソニックＩｐマネジメント株式会社 | Robot control method and robot control device |
-
1985
- 1985-04-22 JP JP8463785A patent/JPS61243513A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022102464A1 (en) * | 2020-11-11 | 2022-05-19 | パナソニックＩｐマネジメント株式会社 | Robot control method and robot control device |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JPH07295650A (en) | Method for controlling articulated robot | |
| US5105135A (en) | Feedback controller for NC controlled machine tools | |
| AU4161200A (en) | Equipment for controlling an industrial robot and a method for programming and/or adjusting the movements of the robot | |
| JPS58124907A (en) | Positioning device for machine tools | |
| CA2042044A1 (en) | Manipulator using plane pulse motor | |
| US4916636A (en) | System for controlling acceleration and deceleration of horizontally articulated robot | |
| JPS61243514A (en) | Robot controller | |
| JPS61243513A (en) | Robot controller | |
| JPS6254302A (en) | Robot controller | |
| GB2166841A (en) | Reaction force compensation | |
| JPS61231612A (en) | Controller of robot | |
| JPS61231611A (en) | Controller of robot | |
| JPS618236A (en) | Moving body control unit | |
| JPS6254303A (en) | Robot controller | |
| JPS61221803A (en) | Robot controller | |
| JPH0424178B2 (en) | ||
| JPS61143812A (en) | Control device of robot | |
| JPS59100903A (en) | Servocontrol device of industrial robot | |
| JPH0323064B2 (en) | ||
| JPH04294990A (en) | Control method for robot | |
| Leonhard | Trajectory control of a multi-axes robot with electrical servo drives | |
| JPS62213980A (en) | Structure of multi-joint type robot | |
| JPH01296301A (en) | Method for controlling servo loop of industrial robot | |
| JPH02131887A (en) | Control unit for horizontal revolving arm robot | |
| JPS6446015A (en) | Control apparatus of magnetic bearing |