JPH03105404A - Direct teach system - Google Patents

Abstract

PURPOSE:To obtain a direct teach system moving the tip of a robot with small force by detecting force from an external part in a sensor and generating driving data of a motor in correspondence with the friction of the robot from the detected result. CONSTITUTION:The force sensor 16 is provided at the tip part of the scalar robot, between a hand 14 and a vertical arm 13, for example. The force sensor 16 has the degree of freedom and it gives a signal corresponding to force to a signal processor 17 when force is given to the hand 14 by a hand and the like. Then, it outputs a control signal controlling the motor 15 and the like to a robot controller 18 so that the signal cancels still friction as against a target direction. The robot controller 18 drives the motor by a signal added from the force sensor signal processor 17 at the time of direct teach. Thus, direct teach is attained with small force, and vibration proof and high speed is attained.

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明はスカラロボットにかかり、さらに詳しくはロボ
ットに移動の情報を入力するダイレクトティーチ方式に
関する。DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a SCARA robot, and more particularly to a direct teach method for inputting movement information to a robot.

３．発明の詳細な説明 〔概　　用〕 ロボットに移動情報入力するダイレクトティーチ方式に
関し、 〔従来の技術］ ロボットの発展により、工場などにおいては生産性の向
上を高めるため、このロボットを多く用いている。一般
的にロボットはあらかじめ定められた制御データによっ
て、どのように動くかを決定されており、ユーザすなわ
ちロボット管理者はこのデータを作業が変わる度に人力
しなくてはならない。特に新しく作業を開始する場合に
おいては、ロボッ１・に入力すべきデータを計算して求
めたり、あるいは予めロボットに設けられているセンサ
を用いてそのロボットの移動を手で行って入力する方式
、すなわちダイレクトティーチ方式がある。3. Detailed Description of the Invention [General Purpose] Regarding the direct teaching method for inputting movement information to a robot, [Prior Art] With the development of robots, robots are often used in factories to improve productivity. Generally, how a robot moves is determined by predetermined control data, and the user, that is, the robot administrator, must manually input this data each time the task changes. Particularly when starting a new work, the data to be input into the robot 1 must be calculated, or input by manually moving the robot using sensors installed in the robot. In other words, there is a direct teach method.

スカラロボットの駆動形は、サーボモー夕に減速機を取
り付けた方式とＤＤドライブモータで直接駆動する方式
がある．サーボモータに減速機を取り付けた方式では、
減速機のＷ！擦が大きく、モーターの電流を流さずにロ
ボットの先端を手で押して任意の位置に動かすには、か
なりの力を必要どする。There are two drive types for SCARA robots: one in which a reducer is attached to a servo motor, and the other in which the robot is driven directly by a DD drive motor. In the system where a reducer is attached to a servo motor,
Reducer W! There is a lot of friction, and it requires a considerable amount of force to push the tip of the robot by hand and move it to a desired position without applying electric current to the motor.

前述のダイレクトティーチ方式は、換言するならば人間
がロボットの先端を手で持ち、所定の位置に動かしてそ
の位置を人力させる方式であり、ロボッｉ・の先端を小
さな力で押せば先端が押された方向に動くことが必要で
ある。そのため従来においては、ロボットの先端に例え
ば６軸カセンザを取りつけ、ハイブリッド制御やインビ
ダンス制御を用いたノＪ制御をすることにより、先端が
押された方向に動くことを実現してきた。In other words, the direct teach method described above is a method in which a human holds the tip of the robot in their hand, moves it to a predetermined position, and then manually sets the tip. It is necessary to move in the direction indicated. For this reason, in the past, for example, a 6-axis sensor was attached to the tip of the robot, and the robot was able to move in the direction in which it was pushed by performing NoJ control using hybrid control or impedance control.

さらに詳細に説明すると、ロボットの先端を小さな力で
押すことにより、その押した方向を６軸カセンサが感知
し、そのセンサの出力によってその方向へのモータを回
転させ、押された方向にロボットの先端が移動するよう
にしている。To explain in more detail, by pushing the tip of the robot with a small force, a 6-axis sensor detects the direction of the push, and the sensor output rotates the motor in that direction, causing the robot to move in the pushed direction. The tip is made to move.

〔発明が解決しようとする課理〕[Problems that the invention attempts to solve]

前述した方式においては、移動速度が１０ｃｍ／ｓｅｃ
以下と遅く、さらにはロボットの振動を増幅するように
力制御が作用するので、ロボットが振動しやすくなると
いう問題を有していた。In the method described above, the moving speed is 10 cm/sec.
Furthermore, since the force control acts so as to amplify the vibrations of the robot, the robot tends to vibrate.

例えば少ない力で押したことにより、モータが回転し、
ロボットの先端はそのモータの回転に対応した方向に移
動する。しかしながこの移動したことによって、例えば
センサをおす力が弱くなり、あるいは押している位置が
手から離れたりして圧力が低下し移動しなくて良いとい
う判断となり停止し、手が再度その移動に追従したとき
に圧力が加わり、また加わったことによってモータが回
転し、というようにその繰り返しによって振動してしま
うことがある。またモータが移動させるため、例えば停
止状態においても先端が揺れる等がありこの移動を検出
するエンコーダ等がその振動をも検知し、ダイレクトテ
ィーチ方式によって入力した移動データを再現する場合
、その振動と同しような制御をも行ってし７まうことか
あった。For example, by pressing with a small amount of force, the motor rotates,
The tip of the robot moves in the direction corresponding to the rotation of its motor. However, due to this movement, for example, the force with which you press the sensor becomes weaker, or the position you are pressing moves away from your hand, the pressure decreases, and it is determined that there is no need to move, and the hand stops moving again. When the object is tracked, pressure is applied, which causes the motor to rotate, and this repetition can cause vibration. In addition, since the motor moves, the tip may shake even when stopped, and the encoder, etc. that detects this movement also detects this vibration, and when reproducing the movement data input by the direct teach method, it is the same as the vibration. In some cases, such control was performed.

本発明は、小さい力で入力すべきロボットの先端を移動
でき、また振動も少なく移動速度も早いダイレクトティ
ーチ方式を目的とする。The present invention aims at a direct teach method that allows the tip of the robot to be moved with a small force and has low vibration and high moving speed.

〔課題を解決するための手段〕[Means to solve the problem]

第１図は本発明の原理ブロック図である。本発明は外部
からの力によるロボットの先端の移動をエンコーダ等の
検出手段１で検出し、前記検出データをもとにモータ駆
動回路５がモータ２を駆動して移動を再現するロボット
におけるものである。FIG. 1 is a block diagram of the principle of the present invention. The present invention relates to a robot in which movement of the tip of the robot due to an external force is detected by a detection means 1 such as an encoder, and a motor drive circuit 5 drives a motor 2 based on the detected data to reproduce the movement. be.

センサ３は前記外部からの力を検出するセンサであり、
例えば６軸カセンサである。制御装置４は前記センサ３
の検出結果から、前記ロボットの静止摩擦を打ち消すよ
う前記モーク２の駆動データを発生する． （作　　　用〕 ロボットに対しロボットの移動データを入力ずる時、ロ
ボッ１・の先端に手等によって力を加え移動する方向に
対し力を加える。この手等の外部からの力をセンサ３は
検出し制御装置４はそのセンサに加わった力の方向に対
応ずるモータをドライブすべき駆動データを発生する。The sensor 3 is a sensor that detects the external force,
For example, it is a 6-axis sensor. The control device 4 is connected to the sensor 3
Based on the detection results, drive data for the moke 2 is generated to cancel the static friction of the robot. (Function) When inputting robot movement data to the robot, a force is applied to the tip of the robot 1 with a hand or the like in the direction of movement.The sensor 3 detects this external force from the hand or the like. The control device 4 then generates drive data to drive the motor corresponding to the direction of the force applied to the sensor.

この駆動データによりモータ駆動回路５はモータ２をド
ライブする。制御装置４が発生する駆動データは外部か
ら加わった力と同一方向にロボットの先端を移動するも
のであり、且つロボットの静止摩擦を打ち消すドライブ
であるので、加わった力に対応して先端は移動すること
となる。The motor drive circuit 5 drives the motor 2 based on this drive data. The drive data generated by the control device 4 moves the tip of the robot in the same direction as the force applied from the outside, and is a drive that cancels out the static friction of the robot, so the tip moves in response to the applied force. I will do it.

小さな力を加えてその力方向に対応してロボットの先端
を移動させるのではなく、力の加わった方向に対応する
静止摩擦力を打ち消すようモータをドライブするので、
その移動においてはあくまでも加えた力によって移動す
るものであり、その力を強くすれば高速に移動させるこ
とができ、さらにはロボットが振動しにくくなり安定し
た移動データを入力することができる。Rather than applying a small force and moving the tip of the robot in response to the direction of the force, the motor is driven to cancel out the static friction force that corresponds to the direction of the force.
The movement is based solely on the applied force, and by increasing the force, the robot can move faster, and the robot is less likely to vibrate, allowing stable movement data to be input.

〔実　　施　　例〕 以下図面を用いて本発明を詳細に説明する．第２図は本
発明の実施例の構成図である。主軸１０の上端部に第１
アーム１ｌが設けられ、また第ｌアーム１１の他端には
第２アームｌ２の一端が設けられている．そして第２ア
ーム１２の他端には上下アーム１３が設けられている．
土軸１０、第１アームｌ１、第２アーム１２、上下アー
ム１３にはそれぞれ自由度が設けられており、この自由
度によって上下アーム１３の下方に設けられたハンドｌ
４を目的の位置にさらに目的の方向にセフトすることが
できる。第３図はカロボットの軸の説明図である。主軸
１０の円筒よりなる中心軸に１軸を中心とし第１アーム
１１が回転する。また第１アームの他端には第２アーム
１２と２軸を中心に回転する構造となっている。そして
第２アーム１２の他端には上下アーム１３が上下方向に
可動となる３軸がありさらに上下アームｌ３の円筒形の
中心軸に４軸が設けられ、ハンド１４はこの４軸を中心
に回転することができる．図示しないが各軸（ｌ軸，２
軸，３軸．４軸）にはロータリエンコーダが設けられお
り、このロータリエンコーダによって現在の軸の位置が
わかる。[Example] The present invention will be explained in detail below using the drawings. FIG. 2 is a block diagram of an embodiment of the present invention. At the upper end of the main shaft 10, a first
An arm 1l is provided, and the other end of the l-th arm 11 is provided with one end of a second arm l2. An upper and lower arm 13 is provided at the other end of the second arm 12.
The soil shaft 10, the first arm l1, the second arm 12, and the upper and lower arms 13 each have a degree of freedom, and this degree of freedom allows the hand l provided below the upper and lower arms 13 to move freely.
4 can be further shifted to the desired position and in the desired direction. FIG. 3 is an explanatory diagram of the axis of the carobot. The first arm 11 rotates about one axis around the cylindrical central axis of the main shaft 10. The other end of the first arm has a structure that rotates about two axes with a second arm 12. The other end of the second arm 12 has three axes that allow the vertical arm 13 to move in the vertical direction, and the cylindrical center axis of the vertical arm l3 has four axes, and the hand 14 moves around these four axes. It can be rotated. Although not shown, each axis (l axis, 2
Axis, 3 axes. A rotary encoder is provided on each of the four axes), and the current position of the axis can be determined by this rotary encoder.

一方ハンド１４の中心軸４軸にはモータ１５がもけられ
ており、このモータｌ５によってハンド１４が回転する
。さらには１軸．２軸，３軸においても同様にモータが
設けられれ、このモータの駆動によってアームが運動す
る。On the other hand, a motor 15 is attached to four central axes of the hand 14, and the hand 14 is rotated by this motor 15. Even one axis. Motors are similarly provided for the two axes and the three axes, and the arms move by driving these motors.

まとめると本発明の実施例においては回転する！．２．
４軸と上下方向に直進する３軸で構威されている。この
自由度は空間ＸＹＺ座標の３直線自由度と２軸上の１回
転自由度の計４自由度を備えている。この自由度とロボ
ットの軸間には、（１）ロボットの４軸はＺ軸回りの回
転自由度（２）ロボットの３軸はＺ方向の自由度（３）
ロボットの１．２軸は連動してい＜ＸＹ方向の直進２自
由度 である。このスカラロボットの先端部、例えば第２図に
おけるハンドｌ４と上下アームｌ３の中間部にカセンサ
１６を設けている。カセンサ１６は６自由度のセンサで
あり、ハンドｌ４を目的の方向に手等によって力を加え
た場合、その加えた力に対応した信号がカセンサ信号処
理装置１７に加わる．カセンサ信号処理装置ｌ７はカセ
ンサｌ６から加わる信号を処理し、目的の方向に対する
静止摩擦に対応して、例えばこの摩擦を打ち消すようモ
ータｌ５等を制御する制御信号をロボット制御装置１８
に出力する。ロボット制御装置１８はカセンサ信号処理
装置１７から加わる信号によりダイレクトティーチ時に
はモータをドライブする．ロボット制御装置１８内には
各モータ第１アーム１１用モータ、第２アーム１２用モ
ータ、上下アーム１３用モータ、ハンドセット回転用モ
ータｌ５をドライブするパワーアンプＩ９を有しており
、それぞれのモータをこのパワーアンプ１９によって制
御する。パワーアンブｌ９は電圧が加わった時にその電
圧に対応する電流を流す電圧電流変換回路であり、カセ
ンサ信号処理装置１７から加わるデジタルデータをロボ
ット制御回路１８内において電圧に変換しパワーアンプ
に加える。パワーアンプは電流を流すアンプであり、そ
の電圧に対応した電流を流す。この流す電流は例えばモ
ータ１５がその方向に回転や移動する時に静止摩擦を打
ち消す駆動であり、ハンド１４をある方向に押し出した
場合、それぞれのモータが静止摩擦を打ち消しているの
で、小さい力でハンドｌ４を目的の位置に移動させるこ
とができる。In summary, in the embodiment of the present invention, it rotates! ．． 2.
It consists of 4 axes and 3 axes that move straight up and down. This degree of freedom has a total of four degrees of freedom, including three linear degrees of freedom in spatial XYZ coordinates and one rotational degree of freedom on two axes. Between this degree of freedom and the robot's axes, (1) the robot's 4 axes have a rotational degree of freedom around the Z axis (2) the robot's 3 axes have a degree of freedom in the Z direction (3)
The robot's 1.2 axes are interlocked and have 2 degrees of freedom for linear movement in the X and Y directions. A force sensor 16 is provided at the tip of this SCARA robot, for example, at the intermediate portion between the hand l4 and the upper and lower arms l3 in FIG. The force sensor 16 is a six-degree-of-freedom sensor, and when a force is applied by hand or the like to the hand l4 in a desired direction, a signal corresponding to the applied force is applied to the force sensor signal processing device 17. The force sensor signal processing device 17 processes the signal applied from the force sensor 16, and sends a control signal to the robot control device 18, which controls the motor 15, etc., to cancel static friction in the target direction, for example, to cancel this friction.
Output to. The robot control device 18 drives the motor during direct teaching using a signal applied from the sensor signal processing device 17. The robot control device 18 includes a power amplifier I9 that drives the first arm 11 motor, the second arm 12 motor, the upper and lower arm 13 motors, and the handset rotation motor l5. It is controlled by this power amplifier 19. The power amplifier 19 is a voltage-current conversion circuit that flows a current corresponding to the voltage when a voltage is applied. Digital data applied from the sensor signal processing device 17 is converted into a voltage within the robot control circuit 18, and the voltage is applied to the power amplifier. A power amplifier is an amplifier that allows current to flow, and it flows a current that corresponds to the voltage. This flowing current is a drive that cancels out static friction when the motor 15 rotates or moves in that direction, for example, and when the hand 14 is pushed in a certain direction, each motor cancels out the static friction, so the hand can be moved with a small force. l4 can be moved to the desired position.

前述ではシステムの基本動作について説明したが、以下
ではさらに第３図に表す４自由度の場合のそれぞれの各
モータの制御について説明する。Although the basic operation of the system has been explained above, the control of each motor in the case of four degrees of freedom shown in FIG. 3 will be further explained below.

本発明の実施例を動作させる場合、その前にま？ロボッ
トの各軸のモータの摩擦による消費する電流を計測する
（それぞれの電流値を工軸〜４軸に対しＩ■＋　　’　
ｒ２＋　　Ｉ　ｒ３＋　　Ｉ　ｒ４とする）。Before operating the embodiment of the present invention, what should I do? Measure the current consumed by the friction of the motor of each axis of the robot (each current value is I■+' for the engineering axis to 4 axes)
r2+ I r3+ I r4).

次に先端の重力に対抗するため３軸のモータが消費する
電流を計測しＩ９３とする。そして動作させる場合まず
ロボットの３軸に、重力に対抗する方向に電流１ｅ３を
流す。この状態でロボットは静止しており、この時のカ
センサ１６の出力をＸ，Ｙ，　　Ｚ方向とＺ軸回りの回
転についてＦ■＋Ｆｌ’！／＋Ｍ　ｒ　ｔとする。この
後ロボットの先端を人間が手で押した時のカセンサの出
力をそれぞれＦ．，Ｆ，，Ｍ．とする。Next, the current consumed by the three-axis motor to counteract the gravity at the tip is measured and defined as I93. When operating the robot, a current 1e3 is first applied to the three axes of the robot in a direction that opposes gravity. In this state, the robot is stationary, and the output of the sensor 16 at this time is F■+Fl'! in the X, Y, and Z directions and rotation around the Z axis. /+M r t. After that, when a human presses the tip of the robot with his/her hand, the output of the Kasensor is recorded as F. ,F,,M. shall be.

前述の電流並びにカセンサの出力に対し、以下のように
行いそれぞれの方向に対し小さい力で回転や移動をさせ
ることができる。With respect to the above-mentioned current and output of the force sensor, rotation and movement can be performed in each direction with a small force by performing the following procedure.

（１）ロボットの４軸はＺ軸回りの回転自由度のＭ．−
Ｍ，ｚ＞Ｏの場合 この時Ｚ軸回りに十方向に回転することを指示している
ので、４軸のモータに電流Ｉｆ４を流す。(1) The four axes of the robot are the rotational degrees of freedom around the Z axis. −
If M, z>O, since rotation in ten directions around the Z axis is instructed at this time, current If4 is applied to the four-axis motor.

これにより先端は小さいほうで十方向に回転でき？。This allows the tip to rotate in ten directions with the smaller end? .

■Ｍ，−Ｍ，，＜Ｏの場合 これはＺ軸回りに一方向に回転すること指示しているの
で、４軸のモータに電流一Ｉ■を流す。■When M, -M,, <O This indicates rotation in one direction around the Z axis, so a current of 1I is applied to the 4-axis motor.

これにより先端は小さい力で一方向に回転することがで
きる。This allows the tip to rotate in one direction with little force.

（２）ロボットの３軸はＺ軸方向の自由度■Ｆ，−Ｆ，
，ｔ＞Ｏの場合 これはＺ軸上方向に移動することを指示しているので、
３軸のモータに電流Ｉ，ｆｆ＋−１ｆ３を流す。(2) The robot's three axes have degrees of freedom in the Z-axis direction ■F, -F,
, t>O, this indicates movement in the upward direction of the Z axis, so
Current I, ff+-1f3 is applied to the three-axis motor.

これにより先端は小さい力でＺ軸上方向に移動すること
ができる。This allows the tip to move upward on the Z-axis with a small force.

■Ｆ，−Ｆ，，＜Ｏの場合 これはＺ軸下方向に移動することを指示しているので、
３軸のモータに電流１．３〜■ｆ３を流す。■If F, -F,,<O This indicates to move downward on the Z axis, so
A current of 1.3 to ■f3 is applied to the 3-axis motor.

これにより先端は小さい力でＺ軸方向に移動することが
できる。This allows the tip to move in the Z-axis direction with a small force.

（３）ロボットの１．２軸は連動してＸ，Ｙ方向の直進
２自由度 第４図はロボットを上から見た角度の説明図である。(3) The 1.2 axes of the robot are interlocked and move straight in the X and Y directions with 2 degrees of freedom. FIG. 4 is an explanatory diagram of the angle when the robot is viewed from above.

θａ，θｄを指定すると一時的には１軸を十方向に回転
すれば先端はθａ＋π／２の方向に移動し、１軸を１方
向に回転すれば先端はθａ一π／２の方向に移動する。If you specify θa and θd, temporarily if you rotate one axis in ten directions, the tip will move in the direction of θa + π/2, and if you rotate one axis in one direction, the tip will move in the direction of θa - π/2. do.

同様に２軸を十方向に回転すれば先端はθｂ＋π／２の
方向に移動し、１軸を１方向に回転させれば先端はθｂ
一π／２の方向に移動する。またセンサの出力を次によ
うに分割し角度を指定する。Similarly, if you rotate two axes in ten directions, the tip will move in the direction of θb + π/2, and if you rotate one axis in one direction, the tip will move in the direction of θb
Move in the direction of -π/2. Also, divide the output of the sensor as follows and specify the angle.

イ　Ｘ軸＋方向だけ　　　　　角度Ｏ 口　Ｘ軸十方向とＹ軸士方向　角度π／４ハ　Ｙ軸十方
向だけ　　　　　角度π／２二　Ｘ軸一方向とＹ軸」一
方向　角度３π／４ホ　Ｘ軸一方向だけ　　　　　角度
π へ　Ｘ軸一方向とＹ軸一方向　角度５π／４ト　Ｙ軸一
方向だけ　　　　　角度３π／２チ　Ｘ軸＋方向とＹ軸
一方向　角度７π／４これらの角度で指示されたものと
、θａ十π／２、θａ　−　π／　２、θｂ　十π／　
２、θｂ　−　ｘ　／　２を比較し、最も近い角度に対
応したモータ電流を流す。その電流値は θａ　＋　ｘ　／　２のとき　１軸に　Ｉｆｆθａ一π
／２のとき　ｌ軸に−ｉｒ＋ θｂ＋π／２のとき　２軸に　Ｉｆ２ θｂ一π／２のとき　２軸にーＩｆ２ とする。A X-axis + direction only Angle O Mouth Only in one direction To angle π One direction on the X axis and one direction on the Y axis Angle 5π/4 t Only one direction on the Y axis Angle 3π/2 t One direction on the X axis and one direction on the Y axis Angle 7 π/4 and θa 1π/2, θa − π/2, θb 1π/
2. Compare θb-x/2 and apply the motor current corresponding to the closest angle. The current value is Ifθa - π on one axis when θa + x / 2
When /2, the l-axis has -ir+ θb+π/2, the second axis has If2. When θb-π/2, the second axis has -If2.

これによりロボットの先端は小さい力でＸ，Ｙ平面内を
移動することができる。This allows the tip of the robot to move within the X and Y planes with a small force.

前述した動作により小さい力でロボットの先端を移動さ
せることができる。The above-described operation allows the tip of the robot to be moved with a small force.

Ｘ軸に対し４軸の中心点Ｐと原点Ｏを引いた直線をθａ
、 Ｘ軸に平行で２軸の中心Ｑと４軸の中心Ｐとの角度θｂ
とする。The straight line drawn from the center point P of the 4 axes and the origin O to the X axis is θa
, Angle θb between the center Q of the second axis and the center P of the fourth axis parallel to the X axis
shall be.

またＸ軸とアーム１１（ａ）の挟む角をθ１、アーム１
１とアーム１２い）の挾む角をθ２とする。この時、移
動方向をＭとするならばＸ軸と平行の線Ｘ′とアーム中
）が移動する方向Ｍならびに相互的に移動する方向Ｎと
の角度はそれぞれＸ′とＮがθａ　−　ｘ　／　２、Ｘ
′とＭがθｂ−ｒｃ／？、その反対方向がＸ′とＭがθ
ｂ＋π／２、Ｘ′とＮがθａ＋π／２となる。Also, the angle between the X axis and arm 11(a) is θ1, and arm 1
1 and arm 12) is defined as θ2. At this time, if the moving direction is M, the angle between the line X' parallel to the X axis and the moving direction M (in the arm) and the mutual moving direction N is θa - x / 2.X
' and M are θb-rc/? , the opposite direction is X' and M is θ
b+π/2, X' and N become θa+π/2.

すなわち、前述した動作をさらにまとめると、まず先端
にカセンサ１６を備えたカロボットを準備し、所定のハ
ンドを取り付けた段階で前述のＩｎ，Ｔｒｚ，Ｉｔ■．
ｒｒ４を計測する．ロボットの１軸，２軸のエンコーダ
から第４図のθ．．θ２は容易に計測される．そして θａ＝θ，　＋ＦＬＡＧ　　（θ２）×（ＣＯＳ　″′
（　（ａ２＋ｃ”−ｂ２）／（２ｘａ　ｘｃ））］・・
・（１） θｂ一θ１＋θ２−π　　　　　　・・・（２）ここで
、 ａ，ｂ＝アーム１１、１２の長さ ｃ＝　（ａ”　＋ｂ”　）’／ｚ ＦＬＡＧ　　（θ２　）　＝１（＜θ２〈π）、−１（
π〈θ２〈２π） によってθａ，θｂを求める． これによりθａ＋π／２、θａ一π／２、θｂ十π／２
、θｂ一π／２を求め、カセンサのＸ．Ｙ方向の入力か
ら前述した（３）の条件に従って角度を計算しこれらの
値と比較する。そして、対応する電流値を１，２軸に出
力する。That is, to further summarize the above-mentioned operations, first, a carobot equipped with a capacitor 16 at the tip is prepared, and when a predetermined hand is attached, the above-mentioned In, Trz, It■.
Measure rr4. θ in Fig. 4 from the robot's 1st and 2nd axis encoders. ．． θ2 is easily measured. And θa=θ, +FLAG (θ2)×(COS ″′
((a2+c"-b2)/(2xa xc))]...
・(1) θb - θ1 + θ2 - π ... (2) Here, a, b = length of arms 11, 12 c = (a" + b")'/z FLAG (θ2) = 1 (<θ2〈 π), -1(
Find θa and θb using π〈θ2〈2π). As a result, θa + π/2, θa - π/2, θb + π/2
, θb - π/2, and calculate the X. The angle is calculated from the input in the Y direction according to the condition (3) described above and compared with these values. Then, the corresponding current values are output to the first and second axes.

３．４軸は対応した力がカセンサの人力により判明する
ので、対応した電流を出力する．また教示した点の位置
は各軸のエンコーダの値を読み込み、ｘ，ｙ，ｚ座標に
変換して記憶する。3. For the 4th axis, the corresponding force is determined by the human power of the sensor, so the corresponding current is output. Furthermore, the position of the taught point is determined by reading the encoder values of each axis, converting them into x, y, and z coordinates, and storing them.

前述したように各軸１軸，２軸，４軸にはそれぞれエン
コーダが設けられており、ハンド１４を移動させる時に
各軸で計測されるエンコーダの値から、以後に移動させ
るべきデータを求める．すなわちエンコーダの値を直接
記憶する．エンコーダの値が記憶されていれば、その値
から各軸の角度を計算し、その角度から先端のＸＹＺ座
標とＺ軸まわりの回転角度θを求める（順変換）ことは
できるので、記憶した点の間を直線で移動することは可
能である．そのため、ティーチング時すなわちハンドを
移動した時とほぼ同じ動作を再現することができる。As mentioned above, each of the first, second, and fourth axes is provided with an encoder, and when the hand 14 is moved, the data to be subsequently moved is determined from the encoder values measured on each axis. In other words, the encoder value is directly memorized. If the encoder values are memorized, it is possible to calculate the angle of each axis from the values and obtain the XYZ coordinates of the tip and the rotation angle θ around the Z axis from those angles (forward transformation), so the memorized points can be It is possible to move in a straight line between. Therefore, it is possible to reproduce almost the same motion as during teaching, that is, when moving the hand.

〔発明の効果〕〔Effect of the invention〕

以上述べたように本発明によれば、力制御を用いないの
でロボットの振動を増幅することなく、振動を低減でき
る．また移動速度に制限がないので力制御時より高速に
移動できる． また計算が三角関数と座標変換だけであり、計算量が少
なくてすむ．As described above, according to the present invention, since force control is not used, vibrations of the robot can be reduced without amplifying them. Also, since there is no limit to movement speed, it can move faster than when using force control. In addition, the calculations are only trigonometric functions and coordinate transformations, so the amount of calculations is small.

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

第１図は本発明の原理ブロック図、 第２図は本発明の実施例の構或図、 第３図はカロボットの軸の説明図、 第４図はロボットを上から見た角度の説明図である． エンコーダ、 モータ、 カセンサ、 制御装置、 モータ駆動回路． 本楚８月の厚理ブロック団 第１図 ／ 埠２′７−ム１２ １４！’−ｑ［１，Ｑ　Ｑ＄，，ｋ４クｉｎ？Ａ戒図ス
カラロボ：、７１の軸の説Ｂ月図Fig. 1 is a block diagram of the principle of the present invention, Fig. 2 is a structural diagram of an embodiment of the present invention, Fig. 3 is an explanatory diagram of the axis of the carobot, and Fig. 4 is an explanation of the angle when the robot is viewed from above. This is a diagram. Encoder, motor, sensor, control device, motor drive circuit. Honchu August Atsuri Block Group Figure 1/Bu 2'7-mu 12 14! '-q[1,Q Q$,,k4ku in? A precept map Scalar Robo:, 71 axis theory B moon map

Claims (1)

【特許請求の範囲】[Claims] 外部からの力によるロボットの先端の移動を検出手段（
１）で検出し、該検出データをもとにモータ駆動回路（
５）がモータ（２）を駆動して移動を再現するロボット
において、前記外部からの力を検出するセンサ（３）と
、該センサ（３）の検出結果から、前記ロボットの摩擦
に対応して前記モータ（２）の駆動データを発生する制
御装置（４）とよりなることを特徴とするダイレクトテ
ィーチ方式。A means for detecting movement of the tip of the robot due to external force (
1), and based on the detected data, the motor drive circuit (
5) is a robot that reproduces movement by driving a motor (2), and a sensor (3) that detects the external force, and a sensor (3) that detects a force corresponding to the friction of the robot based on the detection result of the sensor (3). A direct teach method characterized by comprising a control device (4) that generates drive data for the motor (2).