JP3316967B2 - Robot control device - Google Patents

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【産業上の利用分野】本発明は、軸の回りの重力トルク
を補償するためのバランサを有したロボットにおいて、
各軸の回りの慣性モーメント及び重力トルクがロボット
の姿勢の変化に伴って変化することによって生じる軌跡
制御性能の低下を防止した装置に関する。BACKGROUND OF THE INVENTION The present invention relates to a robot having a balancer for compensating a gravitational torque around an axis.
The present invention relates to a device that prevents a decrease in trajectory control performance caused by a moment of inertia and a gravitational torque around each axis changing with a change in a posture of a robot.

【０００２】[0002]

【従来の技術】多関節ロボットにおいては、軸の回りの
重力トルクを補償するためにバランサを取り付けたもの
が存在する。このバランサを取り付けたロボットにおい
ても、姿勢変化に伴って、各軸の回りの慣性モーメント
及び重力トルクが変化する。このため、位置及び速度フ
ィードバックループのゲインを一定とすると、ロボット
の全作動領域において位置決め誤差や位置決め速度を一
定にすることは困難である。例えば、位置や速度のフィ
ードバックループのゲインを慣性モーメントや重力トル
クが最大となる姿勢の時に所定の位置決め誤差及び位置
決め速度が得られるように設定した場合には、慣性モー
メントや重力トルクが小さくなる姿勢では、フィードバ
ックループのゲインが高くなり過ぎ、振動が発生し、位
置決め時間が長くなるという問題がある。逆に、フィー
ドバックループのゲインを慣性モーメントや重力トルク
が最小となる姿勢の時に所定の位置決め誤差及び位置決
め速度が得られるように設定した場合には、慣性モーメ
ントや重力トルクが大きくなる姿勢では、フィードバッ
クループのゲインが不足し、位置決め誤差が増大した
り、追随遅れが増大し、位置決め時間が長くなるという
問題がある。2. Description of the Related Art Some articulated robots have a balancer attached to compensate for gravitational torque around an axis. Even in a robot equipped with this balancer, the moment of inertia around each axis and the gravitational torque change as the posture changes. For this reason, if the gain of the position and speed feedback loop is fixed, it is difficult to make the positioning error and the positioning speed constant in the entire operation region of the robot. For example, if the gain of the position and speed feedback loop is set so that a predetermined positioning error and positioning speed can be obtained when the inertia moment and the gravitational torque are at the maximum, the posture at which the inertia moment and the gravitational torque become small Then, there is a problem that the gain of the feedback loop becomes too high, vibration occurs, and the positioning time becomes long. Conversely, if the gain of the feedback loop is set so that a predetermined positioning error and positioning speed can be obtained when the inertia moment and the gravitational torque are minimized, the feedback is not performed when the inertia moment and the gravitational torque are increased. There is a problem that the gain of the loop is insufficient, the positioning error increases, the tracking delay increases, and the positioning time increases.

【０００３】この課題を解決するために、教示データ間
を補間して各軸の指令角度を演算すると共にその指令角
度における各軸の回りの慣性モーメント及び重力トルク
を演算して、それらの値に基づいて位置及び速度フィー
ドバックループのゲインを変化させることが考えられ
る。In order to solve this problem, a command angle of each axis is calculated by interpolating between teaching data, and an inertia moment and a gravitational torque around each axis at the command angle are calculated. It is conceivable to change the gain of the position and speed feedback loop based on this.

【０００４】[0004]

【発明が解決しようとする課題】しかしながら、軸の回
りの重力トルクを補償するためのバランサを用いたロボ
ットでは、このバランサによる重力トルクの補償量がロ
ボットの姿勢によって変化する。従って、軸の回りの重
力トルクを演算する場合にも、補償量を考慮に入れる必
要があり、しかも、姿勢によって補償量が変化すること
も考慮に入れなけ れば、サーボ制御性能が向上しない。
そこで、本発明の第１の目的は、バランサによる重力ト
ルクの補償を考慮に入れて、そのトルクに応じた制御を
行うことで、制御性能を向上させることにある。さら
に、教示データの補間には多大な時間がかかる上に、各
補間点における慣性モーメント及び重力トルクを演算す
ると、ＣＰＵの演算時間が長くなり、教示データの補間
周期が長くなる。この結果、ロボットの姿勢変化速度を
大きくすることができないという問題が発生する。 However, the rotation of the shaft
Using a balancer to compensate for gravitational torque
The compensation amount of the gravitational torque by this balancer
It depends on the bot's attitude. Therefore, the weight around the axis
When calculating force torque, it is necessary to take the amount of compensation into account.
And the amount of compensation changes depending on the posture
Consideration to put such only lever also, the servo control performance does not improve.
Therefore, a first object of the present invention is to provide a gravity
Take into account the compensation of the torque and take control according to the torque.
By doing so, control performance is improved. Further
In addition, the interpolation of the teaching data takes a lot of time, and when calculating the moment of inertia and the gravitational torque at each interpolation point, the calculation time of the CPU becomes longer, and the interpolation cycle of the teaching data becomes longer. As a result, there is a problem that the posture change speed of the robot cannot be increased.

【０００５】又、慣性モーメント及び重力トルクの演算
量を減少させる方法として、特開昭５９−２２０８０６
号公報に記載のものが存在する。この方法は、位置及び
姿勢の教示時に、教示点での慣性モーメント及び重力ト
ルクを演算して、教示データと共に記憶しておき、ロボ
ットの動作時には、教示点間の補間演算と共に慣性モー
メント及び重力トルクも補間演算するという方法であ
る。この方法によれば、教示点での慣性モーメント及び
重力トルクを各軸の角度から演算する必要がないことか
ら、ＣＰＵの演算時間は短縮され、教示データの補間周
期が長くならないという利点がある。As a method of reducing the amount of calculation of the moment of inertia and the gravitational torque, Japanese Patent Application Laid-Open No. 59-220806 discloses a method.
There are those described in Japanese Patent Publication No. This method calculates the moment of inertia and the gravitational torque at the teaching point when teaching the position and orientation, and stores them together with the teaching data. Is also a method of performing an interpolation operation. According to this method, since it is not necessary to calculate the moment of inertia and the gravitational torque at the teaching point from the angles of the respective axes, there is an advantage that the calculation time of the CPU is reduced and the interpolation cycle of the teaching data is not lengthened.

【０００６】しかしながら、上記のように、教示時に教
示点における慣性モーメント及び重力トルクを演算する
という方法は、教示時において、ロボットの手首部先端
に取り付けられるハンドやこのハンドが把持する工具の
重心や質量が既知であることが必要となるため、工具を
動作時と同一工具にしておかなければならないという問
題がある。又、別の工具を用いて同一教示データで加工
する場合には、各教示点における慣性モーメント及び重
力トルクが異なるため精度の高い制御を行うことが困難
である。さらに、異なる重量の物体をロボットにより把
持して同一経路で移動させる様な場合には、重量が異な
れば、同一姿勢でも慣性モーメント及び重力トルクが異
なるので、それぞれの教示データを作成する必要があ
る。又、重量物の把持前と把持後では、同一姿勢でも慣
性モーメント及び重力トルクが異なるため、単一の教示
データでは対応できないという問題がある。そこで、本
発明の第２の目的は、動作時において教示データの補間
周期を長くすることがなく、各軸の重力トルクを演算し
た上で、そのトルクに応じた制御を行うことである。 [0006] However, as described above, the method of calculating the moment of inertia and the gravitational torque at the teaching point at the time of teaching involves a hand attached to the tip of the wrist of the robot and the center of gravity of the tool held by the hand at the time of teaching. Since the mass needs to be known, there is a problem that the tool must be kept the same as the one at the time of operation. Further, when machining with the same teaching data using another tool, it is difficult to perform highly accurate control because the inertia moment and the gravitational torque at each teaching point are different. Furthermore, when objects having different weights are gripped by a robot and moved on the same path, if the weights are different, the moment of inertia and the gravitational torque are different even in the same posture, so it is necessary to create respective teaching data. . In addition, since the moment of inertia and the gravitational torque are different before and after holding the heavy object even in the same posture, there is a problem that a single teaching data cannot be used. So the book
A second object of the present invention is to interpolate teaching data during operation.
Calculate the gravitational torque of each axis without lengthening the cycle.
Then, control according to the torque is performed.

【０００８】[0008]

【課題を解決するための手段】上記第1の目的を達成す
るための発明の構成は、軸にかかる重力トルクを減少さ
せるためのバランサを有したロボットの制御装置であっ
て、位置フィードバックループ、速度フィードバックル
ープ及び電流フィードバックループを有し、前記ロボッ
トの工具を指令された位置及び姿勢に追随させる制御装
置において、ロボットの各軸の回転角を検出する角度検
出手段と、前記角度検出手段により検出された各軸の回
転角とロボットの各リンクの長さ及び質量とから慣性モ
ーメント及び重力トルクを演算するモーメント／トルク
演算手段と、前記角度検出手段により検出された各軸の
回転角、及び、バランサの構造に基づいた定数から前記
バランサの取り付けられている軸の回りの前記バランサ
による補償トルクを演算する補償トルク演算手段と、前
記モーメント／トルク演算手段により演算された重力ト
ルクと前記補償トルク演算手段により演算された補償ト
ルクとを合成するトルク合成手段と、前記モーメント／
トルク演算手段により演算された慣性モーメントに基づ
いて前記速度フィードバックループのゲインを変化させ
る第１補正手段と、前記トルク合成手段により演算され
た合成トルクに基づいて前記電流フィードバックループ
への指令値を加算補正する第２補正手段とから構成した
ことである。又、第1の目的に加え、第2の目的を達成す
る発明の構成は、前記モーメント／トルク演算手段及び
補償トルク演算手段を、前記工具が指令された位置への
移動経路を決定するために周期的に実行される補間点演
算の間の空時間を用いて実行するようにしたことであ
る。 [MEANS FOR SOLVING THE PROBLEMS] To achieve the above first object.
The present invention provides a control device for a robot having a balancer for reducing a gravitational torque applied to an axis, comprising a position feedback loop, a speed feedback loop, and a current feedback loop, and a control apparatus for following the directive position and orientation, and angle detecting means for detecting a rotation angle of each axis of the robot, the length of each link of the rotation angle and the robot axes detected by said angle detecting means and and moment / torque calculating means for computation of the mass Toka et moment of inertia and gravity torque, the rotation angle of each axis detected by the angle detection means, and, before Symbol balancer constant based on the structure of the balancer a compensation torque calculating means for calculating a compensation torque by the balancer around on the installed shaft, the moment / DOO A torque combining means for combining the computed compensation torque by gravity torque and the compensation torque calculation means calculated by click operation means, the moment /
First correction means for changing the gain of the speed feedback loop based on the moment of inertia calculated by the torque calculation means; and a command value to the current feedback loop based on the combined torque calculated by the torque synthesis means. And a second correcting means for correcting the temperature. To achieve the second objective in addition to the first objective
The present invention is further characterized in that the moment / torque calculating means and
Compensating torque calculating means is used to move the tool to a commanded position.
Interpolation points performed periodically to determine the travel path
That the execution is performed using the idle time between calculations.
You.

【０００９】[0009]

【作用】第1の目的を達成する構成により、角度検出手
段により検出された各軸の回転角とロボットの各リンク
の長さ及び質量とから慣性モーメント及び重力トルクを
演算し、角度検出手段により検出された各軸の回転角、
及び、バランサの 構造に基づいた定数から前記バランサ
の取り付けられている軸の回りの前記バランサによる補
償トルクを演算する。これら重力トルクと補償トルクか
ら合成トルクが演算され、慣性モーメントに基づいて前
記速度フィードバックループのゲインを変化させ、合成
トルクに基づいて前記電流フィードバックループへの指
令値を加算補正する。 又、ロボットの動作時には、動作
プログラムに従って指定の教示点への指定された移動が
行われる。この時、教示点間における各軸の角度の指令
値は、教示データの補間によってある補間周期毎に演算
される。[Function] An angle detecting means is provided by a structure for achieving the first object.
Rotation angle of each axis detected by step and each link of robot
Moment of inertia and gravitational torque from the length and mass of
Calculated, the rotation angle of each axis detected by the angle detection means,
And from the constant based on the structure of the balancer,
Supplemented by the balancer around the axis on which the
Calculate the compensation torque. These gravitational torque and compensation torque
The resultant torque is calculated from the
Change the speed feedback loop gain and combine
Finger into the current feedback loop based on torque
Add and correct the quotation. When the robot operates, the designated movement to the designated teaching point is performed according to the operation program. At this time, the command value of the angle of each axis between the teaching points is calculated every interpolation cycle by interpolation of the teaching data.

【００１０】この補間周期から補間演算手段による補間
演算に使用される時間を引いた残りの空時間を利用し
て、検出された各軸の回転角とロボットの各リンクの長
さ及び質量とから、順次、各軸の回りの慣性モーメント
及び重力トルクが繰り返し演算される。この順次繰り返
される演算により、その時の各軸の回りの慣性モーメン
ト及び重力トルクが順次更新される。そして、順次、更
新されているその時の各軸の慣性モーメントに基づいて
速度フィードバックループのゲインが変化される。又、
順次、更新されているその時の合成重力トルクに基づい
て電流フィードバックループへの指令値が加算補正され
る。Using the remaining idle time obtained by subtracting the time used for the interpolation calculation by the interpolation calculation means from the interpolation cycle, the detected rotation angle of each axis and the length and mass of each link of the robot are used. The moment of inertia around each axis and the gravitational torque are sequentially calculated. By this sequentially repeated calculation, the moment of inertia around each axis and the gravitational torque at that time are sequentially updated. Then, the gain of the speed feedback loop is sequentially changed based on the updated moment of inertia of each axis at that time. or,
The command value to the current feedback loop is sequentially added and corrected based on the updated synthetic gravity torque at that time.

【００１１】[0011]

【発明の効果】このように、バランサによる軸回りの重
力トルクの補償量も考慮した合成重力トルクを演算して
いるので、姿勢に係わらず制御性能が向上する。 又、モ
ーメント／トルク演算手段による演算は、補間周期から
補間演算手段による補間演算に使用される時間を引いた
残りの空時間に実行され、数回の補間周期を使って演算
が完了されるので、慣性モーメント及び重力トルクの演
算のために補間演算の周期が長くなるということが防止
される。つまり、慣性モーメント及び重力トルクの演算
は上記の残り時間を利用して分割して演算される。この
ような演算を行うことによって、各補間点毎の慣性モー
メント及び重力トルクを演算することはできないが、リ
アルタイムによる演算が可能となるため、ロボット等が
各種のワークを把持する場合でも容易に対応できる。As described above, the weight around the axis by the balancer is
Calculate the synthetic gravity torque taking into account the amount of force torque compensation
Control performance is improved regardless of the attitude. Also, the calculation by the moment / torque calculation means is executed in the remaining idle time obtained by subtracting the time used for the interpolation calculation by the interpolation calculation means from the interpolation cycle, and the calculation is completed using several interpolation cycles. , The period of the interpolation calculation for calculating the moment of inertia and the gravitational torque is prevented from being lengthened. In other words, the calculation of the moment of inertia and the gravitational torque is divided and calculated using the above remaining time. By performing such calculations, it is not possible to calculate the moment of inertia and gravitational torque for each interpolation point, but it is possible to perform calculations in real time, so that it can be easily handled even when a robot or the like grips various workpieces. it can.

【００１２】[0012]

【実施例】以下本発明を具体的な実施例に基づいて説明
する。図１は６軸多関節ロボットの機構を示した機構図
である。１０がロボット本体であり、フロアに本体１０
を固定するベース１３が配設され、ベース１３上にはコ
ラム１２が固設されており、コラム１２はボディ１４を
回転自在に配設している。ボディ１４はアッパーアーム
１５を回動自在に軸支し、アッパーアーム１５は、フォ
アアーム１６を回動自在に軸支している。ボディ１４、
アッパーアーム１５、フォアアーム１６は、それぞれ、
サーボモータSm１，Sm２，Sm３（図２参照）によって、
軸ａ，ｂ，ｃの回りに回転駆動される。この回転角はエ
ンコーダＥ１，Ｅ２，Ｅ３によって検出される。フォア
アーム１６の先端部にはツイストリスト１７がｄ軸の周
りに回転可能に軸支され、ツイストリスト１７にはベン
ドリスト９がｅ軸の周りに回動自在に軸支されている。
ベンドリスト９には先端にフランジ１８ａを有するスイ
ベルリスト１８がｆ軸の回りに回転可能に軸支されてい
る。また、フランジ１８ａにはハンド１９が取り付けら
れている。ｄ軸、ｅ軸、ｆ軸はサーボモータSm４、Sm
５、Sm６によって駆動され、その回転角はエンコーダＥ
４，Ｅ５，Ｅ６によって検出される。ハンド１９の開閉
動作は工具駆動回路２３により制御される。DESCRIPTION OF THE PREFERRED EMBODIMENTS The present invention will be described below based on specific embodiments. FIG. 1 is a mechanism diagram showing a mechanism of a six-axis articulated robot. Reference numeral 10 denotes a robot main body, and the main body 10 is provided on the floor.
Is fixed, and a column 12 is fixed on the base 13, and the column 12 has a body 14 rotatably arranged. The body 14 pivotally supports the upper arm 15, and the upper arm 15 pivotally supports the forearm 16. Body 14,
The upper arm 15 and the forearm 16 are respectively
By the servo motors Sm1, Sm2, Sm3 (see FIG. 2),
It is driven to rotate around axes a, b, and c. This rotation angle is detected by encoders E1, E2, E3. A twist wrist 17 is rotatably supported on the distal end of the forearm 16 around the d-axis, and the bend wrist 9 is rotatably supported on the twist wrist 17 around the e-axis.
A swivel wrist 18 having a flange 18a at the tip is rotatably supported on the bend wrist 9 so as to be rotatable around the f-axis. A hand 19 is attached to the flange 18a. d axis, e axis, f axis are servo motors Sm4, Sm
5, driven by Sm6, the rotation angle of which is encoder E
4, E5 and E6. The opening / closing operation of the hand 19 is controlled by the tool driving circuit 23.

【００１３】軸ｂの回りの重力トルクはロボットの姿勢
によって大きく変化する。この重力トルクの姿勢による
変動を補償するためにコイルスプリングによるバランサ
１１が設けられている。このバラサン１１の一点１１１
はボディ１４において軸ｂを通る鉛直線上に固定されて
いる。又、他点１１２はアッパーアーム１５においてｂ
軸とｃ軸を含む平面上であってｂ軸とｃ軸とを結ぶ線分
ｇ上に固定さている。そして、アッパーアーム１５が鉛
直線上方向から鉛直線下方向に向かって回転するに連れ
て、このバランサ１１は伸びる。従って、アッパーアー
ム１５は軸ｂの回りにバランサ１１の引張力により鉛直
上向きに回転する方向の補償トルクを受ける。この補償
トルクが軸ｂの回りに姿勢によって変化する重力トルク
をある程度補償する。The gravitational torque around the axis b greatly changes depending on the posture of the robot. A balancer 11 using a coil spring is provided to compensate for the fluctuation due to the attitude of the gravitational torque. One point 111 of this Barasan 11
Is fixed on a vertical line passing through the axis b in the body 14. The other point 112 is b
It is fixed on a line g connecting the b-axis and the c-axis on a plane including the axis and the c-axis. The balancer 11 extends as the upper arm 15 rotates from above the vertical line to below the vertical line. Therefore, the upper arm 15 receives a compensation torque in the direction of rotating vertically upward by the tensile force of the balancer 11 around the axis b. This compensation torque compensates to some extent the gravitational torque that changes depending on the attitude around the axis b.

【００１４】図２は、本発明のロボットの制御装置の電
気的構成を示したブロックダイヤグラムである。ＣＰＵ
２０には、メモリ２５、サーボモータを駆動するための
サーボＣＰＵ２２ａ〜２２ｆ、動作開始指令、ジョグ運
転の指令、教示点の指示等を行う操作盤２６が接続され
ている。ロボットに取付けられた各軸ａ〜ｆ駆動用のサ
ーボモータSm１〜Sm６は、それぞれサーボＣＰＵ２２ａ
〜２２ｆによって駆動される。FIG. 2 is a block diagram showing the electrical configuration of the robot controller according to the present invention. CPU
20 is connected to a memory 25, servo CPUs 22a to 22f for driving servo motors, and an operation panel 26 for performing an operation start instruction, a jog operation instruction, a teaching point instruction, and the like. Servo motors Sm1 to Sm6 for driving the axes a to f attached to the robot are respectively provided with servo CPUs 22a
To 22f.

【００１５】サーボＣＰＵ２２ａ〜２２ｆのそれぞれ
は、ＣＰＵ２０から出力される各軸の角度指令値θ1 〜
θ6 、慣性モーメントＤi 、重力トルクＴi に基づい
て、サーボモータSm１〜Sm６の出力トルクを制御する。
各駆動軸に連結されたエンコーダＥ１〜Ｅ６の出力する
検出角度α1 〜α6 はＣＰＵ２０及びサーボＣＰＵ２２
ａ〜２２ｆに入力しており、ＣＰＵ２０による各軸の慣
性モーメント及び重力トルクの演算及びサーボＣＰＵ２
２ａ〜２２ｆによる位置フィードバック制御、速度フィ
ードバック制御、電流フィードバック制御に用いられ
る。Each of the servo CPUs 22a to 22f outputs an angle command value θ1 to an angle command value of each axis output from the CPU 20.
The output torque of the servo motors Sm1 to Sm6 is controlled based on θ6, the moment of inertia Di, and the gravitational torque Ti.
The detection angles α1 to α6 output from the encoders E1 to E6 connected to the respective drive shafts are determined by the CPU 20 and the servo CPU 22.
a to 22f, calculation of the moment of inertia and gravitational torque of each axis by the CPU 20 and the servo CPU 2
It is used for position feedback control, speed feedback control, and current feedback control by 2a to 22f.

【００１６】メモリ２５にはロボットを教示点データに
従って動作させるためのプログラムが記憶されたＰＡ領
域とハンド１９の位置と姿勢を表す教示点データを記憶
するＰＤＡ領域とフランジ１８ａから先の重量物、即
ち、ハンド１９、又は、ハンド１９とそれにより握持さ
れた物体を総合した物体の重心座標（ａ，ｂ，ｃ）とそ
の質量ｍL 、ロボットの各アームの長さ重量、バランサ
１１の幾何学的位置関係及びバネ定数等の慣性モーメン
ト、質量を記憶するＳＤＡ領域とサーボパラメータを記
憶するＩＴＡ領域と補間演算により求められた補間点に
おける各軸の角度指令値θ1 〜θ6 を記憶するＩＮＡ領
域とエンコーダＥ１〜Ｅ６から出力された検出角度α1
〜α6 を記憶するＡＮＧ領域とが形成されている。The memory 25 has a PA area in which a program for operating the robot in accordance with the teaching point data is stored, a PDA area in which teaching point data indicating the position and posture of the hand 19 is stored, a heavy object ahead of the flange 18a, That is, the coordinates of the center of gravity (a, b, c) of the hand 19 or the object obtained by integrating the hand 19 and the object gripped by the hand 19 and the mass ML thereof, the length and weight of each arm of the robot, and the geometry of the balancer 11 An SDA area for storing inertial moments such as a dynamic positional relationship and a spring constant, a mass, an ITA area for storing servo parameters, and an INA area for storing angle command values θ1 to θ6 of respective axes at interpolation points obtained by interpolation calculation. Detection angle α1 output from encoders E1 to E6
And an ANG area for storing .alpha.6.

【００１７】各軸のサーボＣＰＵ２２ａ〜２２ｆは、図
３に示す回路の機能をシーケンシャルなディジタル制御
により達成するものである。即ち、各回転角の位置のフ
ィードバックループと速度のフィードバックループとで
電流のフィードバックループと速度のフィードフォワー
ドループと加速度のフィードフォワードループとを有し
ている。The servo CPUs 22a to 22f for each axis achieve the functions of the circuit shown in FIG. 3 by sequential digital control. In other words, a feedback loop for the position of each rotation angle and a feedback loop for the speed include a current feedback loop, a speed feedforward loop, and an acceleration feedforward loop.

【００１８】次に、本装置の作動について説明する。図
５はＲＡＭ２５のＰＡ領域に記憶されている動作プログ
ラムである。行番号１０、５０、９０が重量物の重心座
標ｒ（ａ，ｂ，ｃ）とその質量ｍL とを設定するための
命令語である。この命令語の実行により重心座標ｒ
（ａ，ｂ，ｃ）と質量ｍL のデータがＲＡＭ２５のＳＤ
Ａ領域に記憶される。命令語の領域Ａが重心座標ｒを表
し、領域Ｂが質量ｍL を表す。重心座標（ａ，ｂ，ｃ）
はスイベルリスト１８のフランジ１８ａの中心に固定さ
れたリンク座標系から見た座標、即ち、ノーマル、オリ
エント、アプローチの各座標（mm単位) で与えられる。
又、質量はkg単位で表記される。行番号１０、９０は、
フランジ１８ａから先の重量物がハンド１９だけの場合
のデータ設定に関する命令語であり、行番号５０はハン
ド１９により物体が握持された場合のデータ設定に関す
る命令語である。又、行番号２０及び８０のＨＡＮＤ
ＯＦＦ命令語は、ハンド１９を開く命令語であり、行番
号４０のＨＡＮＤ ＯＮ命令語はハンド１９を閉じる命
令語である。さらに、行番号３０、６０、７０、１０
０、１１０のＭＯＶＥ命令語は指定された教示点Ｐｎに
移動させるための命令語である。Next, the operation of the present apparatus will be described. FIG. 5 shows an operation program stored in the PA area of the RAM 25. Line numbers 10, 50 and 90 are command words for setting the coordinates r (a, b, c) of the center of gravity of the heavy object and its mass ML. By executing this command, the barycentric coordinates r
(A, b, c) and the data of mass mL are stored in the SD of the RAM 25.
Stored in area A. The area A of the command word indicates the barycenter coordinate r, and the area B indicates the mass ml. Barycentric coordinates (a, b, c)
Are given by coordinates viewed from a link coordinate system fixed to the center of the flange 18a of the swivel list 18, that is, coordinates of normal, orient, and approach (in mm).
The mass is expressed in kg. Line numbers 10, 90 are
A command related to data setting in a case where only the hand 19 is the heavy object ahead of the flange 18a, and a line number 50 is a command related to data setting in a case where an object is gripped by the hand 19. Also, the HAND of line numbers 20 and 80
The OFF command is a command for opening the hand 19, and the HAND ON command on line number 40 is a command for closing the hand 19. Further, line numbers 30, 60, 70, 10
MOVE commands 0 and 110 are commands for moving to the designated teaching point Pn.

【００１９】図５の動作プログラムにより、ロボット
は、ハンド１９を開いて、Ｐ１点に移動し、その後、ハ
ンド１９を閉じて物体を握持し、Ｐ２点を経由してＰ３
点に移動して、ハンド１９を開いて、物体を放置し、Ｐ
４点を経由してＰ５点に移動することが可能である。According to the operation program shown in FIG. 5, the robot opens the hand 19 and moves to the point P1, then closes the hand 19 and grips the object, and moves to the point P3 via the point P2.
Move to the point, open the hand 19, leave the object,
It is possible to move to point P5 via four points.

【００２０】図４は、ＣＰＵ２０による動作プログラム
を解読するための主プログラムのフローチャートであ
る。ステップ１００において、ＭＯＶＥ命令語が解読さ
れると、ステップ１０２において、現在位置から指定さ
れた教示点までハンド１９を移動させるための補間演算
が実行される。そして、補間演算により求められた各軸
の角度指令値θ1 〜θ6 はサーボＣＰＵ２２ａ〜２２ｆ
に出力される。又、ステップ１０４において、ＬＯＡＤ
命令語が解読されると、ステップ１０６において、その
命令語に表記されているハンドから先の重量物の重心座
標と質量のデータがＲＡＭ２５のＳＤＡ領域に記憶され
る。又、ステップ１０８でＨＡＮＤ ＯＦＦ命令語が解
読されると、ステップ１１０において、工具駆動回路２
３にハンド１９を開く指令を与える。又、ステップ１１
２でＨＡＮＤ ＯＮ命令語が解読されると、ステップ１
１４において、工具駆動回路２３にハンド１９を閉じる
指令を与える。FIG. 4 is a flowchart of a main program for decoding the operation program by the CPU 20. When the MOVE command is decoded in step 100, an interpolation calculation for moving the hand 19 from the current position to the designated teaching point is executed in step 102. Then, the angle command values θ1 to θ6 of the respective axes obtained by the interpolation calculation are used as the servo CPUs 22a to 22f.
Is output to In step 104, LOAD
When the command is decoded, the data of the barycentric coordinates and the mass of the preceding heavy object from the hand described in the command are stored in the SDA area of the RAM 25 in step 106. When the HAND OFF command is decoded in step 108, in step 110, the tool driving circuit 2
3 is given an instruction to open the hand 19. Step 11
When the HAND ON command is decoded in step 2, step 1
At 14, a command to close the hand 19 is given to the tool drive circuit 23.

【００２１】ステップ１０２における教示点間の補間演
算は、図６に示すフローチャートに従って実行される。
この補間演算は、例えば、８mS毎の補間周期で実行され
る。補間演算は、良く知られたように、回転主軸法等を
用いて行うことができる（例えば、特開昭６２−１５４
００６号）。ステップ２００において、ワールド座標系
で表記された教示データとして与えられた開始点の位置
及び姿勢と次の位置決め目標点の位置及び姿勢から回転
主軸の方向ベクトルが演算され、ステップ２０２におい
て、その回転主軸回りの回転角Θが演算される。次に、
ステップ２０４において、開始点の位置及び姿勢を基準
とした補間角ΔΘが演算され、次のステップ２０６にお
いて、その補間角ΔΘを用いて開始点の位置及び姿勢を
補間点における位置及び姿勢に変換するための姿勢変換
行列Ｒが演算される。そして、ステップ２０８におい
て、開始点の位置及び姿勢を表す同次座標行列に姿勢変
換行列を作用させて、補間点における位置及び姿勢を表
す同次座標行列が演算される。次に、ステップ２１０に
おいて、その補間点におけるワールド座標系で表記され
た位置及び姿勢の同次座標行列からジョイント座標系で
の値、即ち、各軸の回転角が演算され、この値はＲＡＭ
２５のＩＮＡ領域に記憶される。The interpolation calculation between the teaching points in step 102 is executed according to the flowchart shown in FIG.
This interpolation calculation is executed, for example, at an interpolation cycle of 8 ms. As is well known, the interpolation operation can be performed by using a rotation spindle method or the like (for example, see Japanese Patent Application Laid-Open No. Sho 62-154).
006). In step 200, the direction vector of the rotation spindle is calculated from the position and orientation of the starting point and the position and orientation of the next positioning target point given as the teaching data expressed in the world coordinate system. The rotation angle Θ is calculated. next,
In step 204, the interpolation angle ΔΘ based on the position and orientation of the start point is calculated, and in the next step 206, the position and orientation of the start point are converted into the position and orientation at the interpolation point using the interpolation angle ΔΘ. Is calculated. Then, in step 208, the homogeneous coordinate matrix representing the position and orientation at the interpolation point is calculated by applying the orientation transformation matrix to the homogeneous coordinate matrix representing the position and orientation of the start point. Next, in step 210, the value in the joint coordinate system, that is, the rotation angle of each axis, is calculated from the homogeneous coordinate matrix of the position and orientation represented in the world coordinate system at the interpolation point, and this value is stored in the RAM.
25 are stored in the INA area.

【００２２】この状態で、本補間プログラムは終了し、
ステップ２１２で、後述する優先度が低いレベルに設定
された慣性モーメント及び重力トルク演算プログラムの
再開処理（中断した命令語の記憶アドレスへの分岐命令
及び状態レジスタ等の復元処理）が行われた後、その慣
性モーメント及び重力トルク演算プログラムが継続起動
される。In this state, the interpolation program ends, and
In step 212, after the inertia moment and gravitational torque calculation program set to a low priority level described below is restarted (branch instruction to the storage address of the interrupted instruction word and restoration processing of the status register, etc.) are performed. , The moment of inertia and the gravity torque calculation program are continuously started.

【００２３】一方、８msec毎のタイマ割り込みが発生す
ると（ステップ２１４）、ステップ２１６において、優
先度の低いレベルに設定された慣性モーメント及び重力
トルク演算プログラムへの割込処理（中断した命令語が
記憶されているアドレスの記憶、状態レジスタ等のスタ
ック処理等）が行われ、優先度の高い本補間処理のプロ
グラムが実行さる。そして、ステップ２１２、２１４に
おいて、既に、演算完了してＲＡＭ２５のＩＮＡ領域に
記憶された補間点の角度指令値θ1 〜θ6 、後述する慣
性モーメント及び重力トルク演算プログラムにより演算
されＲＡＭ２５のＩＴＡ領域に記憶されているサーボパ
ラメータＤi,Ｔi がサーボＣＰＵ２２ａ〜２２ｆに出力
される。この結果、各軸はその時の位置及び姿勢に対応
した慣性モーメント及び重力トルクを補償したフィード
フォワード制御によりそれらの角度指令値θ1 〜θ6 に
位置決め制御される。そして、ステップ２１６におい
て、補間点が教示データとして与えられた目標位置に達
したか否かが判定され、補間が終了していない場合に、
次の補間点の位置及び姿勢の演算のために、ステップ２
０６以下が実行される。一方、補間が終了した場合に
は、次の命令語を解読するために図４のメインプログラ
ムに戻る。On the other hand, when a timer interrupt occurs every 8 msec (step 214), in step 216, the process of interrupting the inertia moment and gravitational torque calculation program set to the low priority level (the interrupted instruction is stored) And the stack processing of the status register, etc.) is performed, and the program of the interpolation processing having a high priority is executed. In steps 212 and 214, the angle command values θ1 to θ6 of the interpolation points already completed in the INA area of the RAM 25 and calculated in the INA area of the RAM 25 are calculated by an inertia moment and gravity torque calculation program described later and stored in the ITA area of the RAM 25. The servo parameters Di, Ti are output to the servo CPUs 22a to 22f. As a result, the axes are controlled to their angular command values θ1 to θ6 by feedforward control that compensates for the moment of inertia and gravitational torque corresponding to the position and orientation at that time. Then, in step 216, it is determined whether or not the interpolation point has reached the target position given as the teaching data.
Step 2 is used to calculate the position and orientation of the next interpolation point.
06 and below are executed. On the other hand, when the interpolation is completed, the process returns to the main program of FIG. 4 to decode the next instruction word.

【００２４】上記の処理でステップ２００〜２０４は、
メインプログラムから移行した補間演算の最初に実行さ
れ、ステップ２１４〜２１２は、８mS毎のタイマ割り込
みに同期して実行される。よって、次の出力タンミング
のために演算されている角度指令値θ1 〜θ6 が、８mS
毎にステップ２１８において出力される。そして、ステ
ップ２１４〜２１２は、およそ６mSかかり、残りの２mS
が慣性モーメント／重力トルクの演算に使用される。こ
の２mSでは慣性モーメント／重力トルクの演算が完了し
ないので、１補間周期の空時間２mSを断続的に使用し
て、即ち、数回の補間周期を使用して、約６〜１２mSか
けて、一回の演算が完了する。Steps 200 to 204 in the above processing are:
It is executed at the beginning of the interpolation operation shifted from the main program, and steps 214 to 212 are executed in synchronization with a timer interrupt every 8 ms. Therefore, the angle command values θ1 to θ6 calculated for the next output timing are 8 mS
It is output at step 218 every time. Steps 214 to 212 take about 6 ms, and the remaining 2 ms
Is used for calculating the moment of inertia / gravity torque. In this 2 ms, the calculation of the moment of inertia / gravity torque is not completed, so that 2 mS of the idle time in one interpolation cycle is used intermittently, that is, about 6 to 12 ms using several interpolation cycles. Calculations are completed.

【００２５】次に、この優先度の低いレベルに設定され
ている慣性モーメント及び重力トルクの演算手順につい
て、図７に示すフローチャートを参照して説明する。ス
テップ３００において、ロボットの軸を特定する軸変数
ｉが１に初期設定され、ステップ３０２においてエンコ
ーダＥ１〜Ｅ６から各軸の現在の回転角α1〜α6 が検
出され、その値はＲＡＭ２５のＡＮＧ領域に記憶され
る。次に、ステップ３０４において、ｉ軸の回りの慣性
行列Ｄi ( α1,…, α6)( 慣性モーメントの行列表現）
が演算され、ステップ３０６において、ｉ軸の回りの重
力トルクＴi ( α1,…, α6)が演算される。尚、このス
テップでの重力トルクの演算では、ｂ軸の回りの重力ト
ルクには、バランサ１２による補償トルクが加味されて
いない。又、ｉ軸の回りのトルクτi は、次式で良く知
られたように、ロボットアームに関するラグランジュ関
数の微分から求めることができる。Next, the procedure of calculating the moment of inertia and the gravitational torque set at the lower priority level will be described with reference to the flowchart shown in FIG. In step 300, an axis variable i for specifying the robot axis is initially set to 1. In step 302, the current rotation angles α1 to α6 of each axis are detected from the encoders E1 to E6, and the values are stored in the ANG area of the RAM 25. It is memorized. Next, in step 304, an inertia matrix Di (α1,..., Α6) around the i-axis (matrix expression of moment of inertia)
Is calculated, and in step 306, the gravitational torque Ti (α1,..., Α6) around the i-axis is calculated. In the calculation of the gravitational torque in this step, the compensation torque by the balancer 12 is not added to the gravitational torque around the b-axis. Further, the torque τi around the i-axis can be obtained from the differentiation of the Lagrangian function relating to the robot arm, as is well known by the following equation.

【００２６】[0026]

【数１】τi ＝Σj=inΣk=ijTr(ＵjkＪjＵjiT)D2θk -
ΣｍjｇＵjijｒj 但し、コリオリ力と遠心力の成分は小さいとして上式で
は除外した。Ｊj はｊ軸を構成する部材だけによるリン
ク慣性行列、Trは行列式の対角成分の和、ｍj はｊ軸リ
ンクの質量、ｇは重力加速度、 jｒj はｊ軸リンク座標
系で表されたｊ軸リンクの重心のベクトルであり、D2θ
k は角度の時間に関する２次微分、即ち、角加速度であ
る。又、上式の第１項の展開式において、D2θi の係数
が慣性行列Ｄi となり、第２項がｉ軸の回りの重力トル
クＴi となる。又、Ｕjkは、[Expression 1] τi = Σj = inΣk = ijTr (UjkJjUjiT) D2θk-
ΣmjgUjijrj However, the components of the Coriolis force and the centrifugal force are small and are excluded from the above equation. Jj is a link inertia matrix composed only of members constituting the j-axis, Tr is the sum of diagonal components of the determinant, mj is the mass of the j-axis link, g is the gravitational acceleration, and jrj is j expressed in the j-axis link coordinate system. Vector of the center of gravity of the axis link, D2θ
k is the second derivative of the angle with respect to time, that is, the angular acceleration. In the expansion formula of the first term of the above equation, the coefficient of D2θi becomes the inertia matrix Di, and the second term becomes the gravitational torque Ti about the i-axis. Ujk also

【００２７】[0027]

【数２】０ （ｊ＜ｉの時）2 (when j <i)

【数３】δ（0A11A2…j-1Aj )/δθi ＝Πs=1j s-1A s
・δi-1Ai / δθi （ｊ≧ｉの時） 但し、ｓ≠ｉである。又、j-1Aj はｊ軸リンク座標をｊ
−１軸リンク座標へ変換する同次座標変換行列であり、
0A1 は１軸リンク座標をワールド座標に変換する同次座
標変換行列である。## EQU3 ## δ (0A11A2... J-1Aj) / δθi = Πs = 1js-1As
Δi−1Ai / δθi (when j ≧ i) where s ≠ i. J-1Aj represents the j-axis link coordinate as j
-1 is a homogeneous coordinate conversion matrix for converting to an axis link coordinate,
0A1 is a homogeneous coordinate conversion matrix for converting one-axis link coordinates into world coordinates.

【００２８】Ｊj は慣性テンソルＩijを用いて次の様に
表現することができる。Jj can be expressed as follows using the inertia tensor Iij.

【数４】 (Equation 4)

【００２９】但し、ｒix、ｒiy、ｒizはｊ軸リンク座標
から見たｊ軸リンクの重心の座標であり、ｍiはその質
量である。又、Here, rix, riy, and riz are coordinates of the center of gravity of the j-axis link viewed from the j-axis link coordinates, and mi is its mass. or,

【数５】Ｉij＝∫〔δij〔Σｘk2 〕−ｘiｙi 〕dｍ である。## EQU5 ## Iij = ∫ [δij [−xk2] -xiyi] dm.

【００３０】尚、ＬＯＡＤ命令語によって設定される重
心座標（ａ，ｂ，ｃ）及び質量ｍLにより、Ｊ6 が変化
する。Ｊ6 は、数４において、（ｒix，ｒiy，ｒiz）＝
（ａ，ｂ，ｃ）、ｍi ＝ｍL とおくことにより求めるこ
とができる。Note that J6 changes depending on the barycentric coordinates (a, b, c) and the mass mL set by the LOAD command. J6 is, in equation 4, (rix, riy, riz) =
(A, b, c), and can be obtained by setting mi = mL.

【００３１】数１から、現在の検出角度（α1 ，…，α
6 ）における各軸の回りの慣性行列Ｄi と重力トルクＴ
i とが演算される。本実施例では、演算時間を短縮する
ために、他軸との相互作用は小さいとして、(i,i) 対角
成分のみを考慮している。請求の範囲の慣性モーメント
の用語は、対角成分だけでなく慣性行列を含む意味で用
いているが、以下の記載では、便宜上、慣性行列Ｄi の
(i,i) 対角成分を、慣性モーメントＤi と記す。From Expression 1, the current detection angle (α1,..., Α
6) Inertia matrix Di around each axis and gravitational torque T
i is calculated. In this embodiment, in order to reduce the calculation time, it is assumed that the interaction with other axes is small, and only the (i, i) diagonal component is considered. Although the term of the moment of inertia in the claims is used to mean not only the diagonal components but also the inertia matrix, in the following description, for convenience, the inertia matrix Di
(i, i) The diagonal component is described as moment of inertia Di.

【００３２】次に、ステップ３０７において、ｉ＝２、
即ち、ｂ軸であるならば、ステップ３０８においてにお
いて、バランサ１１によるｂ軸の回りの補償トルクが演
算される。補償トルクの演算方法を次に述べる。バラン
サ１１の幾何学的配置関係は図９に示すとおりである。Next, at step 307, i = 2,
That is, if it is the b-axis, in step 308, the compensation torque around the b-axis by the balancer 11 is calculated. The method of calculating the compensation torque will be described below. The geometric arrangement of the balancer 11 is as shown in FIG.

【００３３】[0033]

【数６】ｌ＝（ｒ2＋ｈ2−２ｒ×ｈ×cosα)1/2## EQU6 ## l = (r2 + h2-2r.times.h.times.cos.alpha.) 1/2

【数７】δ＝ｌ−（ｒ−ｈ）Δ = l− (r−h)

【数８】β＝tan-1｛ｈ sinα／（ｒ−ｈ cosα) ｝Β = tan-1 {h sinα / (r−h cosα)}

【数９】ｆ＝δ×ｋ＋ｆ0## EQU9 ## f = δ × k + f0

【数１０】ｔ2＝ｒ×ｆ×sinβ×ｎT2 = r × f × sinβ × n

【００３４】但し、 ｒ：アッパーアーム１５の長さ（m) ｌ：バランサ１１の長さ（m) ｈ：バランサ１１の取付け高さ（m) ｋ：バネ定数（N/m) ｆ0 ：バネ初期力（N) ｎ：バランサー数 α：ジョイント角θ2 (rad) β：アッパーアーム１５とバランサ１１とのなす角 (ra
d) γ：バランサ１１のたわみ (m) ｆ：バランサ１本当たりの補償力 (N) ｔ2 ：ｂ軸の回りのバランサ１１による補償トルク (N
・m)Here, r: length of upper arm 15 (m) l: length of balancer 11 (m) h: mounting height of balancer 11 (m) k: spring constant (N / m) f0: spring initial Force (N) n: Number of balancers α: Joint angle θ2 (rad) β: Angle between upper arm 15 and balancer 11 (ra
d) γ: deflection of balancer 11 (m) f: compensation force per balancer (N) t2: compensation torque by balancer 11 around b axis (N
・ M)

【００３５】次に、ステップ３１０において、ｉ軸がｂ
軸（ｉ＝２）の場合には、ｂ軸に関してステップ３０６
で得られた重力トルクＴ2 にステップ３０８で得られた
補償トルクｔ2 を加算して、この値を新らしい合成重力
トルクＴ2 とする。Next, in step 310, the i-axis is set to b
In the case of the axis (i = 2), step 306 is performed for the b axis.
The compensation torque t2 obtained in step 308 is added to the gravitational torque T2 obtained in step 308, and this value is used as a new combined gravitational torque T2.

【００３６】ステップ３１２において、慣性モーメント
Ｄi と重力トルクＴi がサーボパラメータとしてＲＡＭ
２５のＩＴＡ領域に更新記憶される。この値が図６のス
テップ２２０においてサーボＣＰＵ２２ａ〜２２ｆに出
力される。次に、ステップ３１０において、軸変数ｉが
最大値６に達したか否かが判定され、その値に達してい
ない場合には、ステップ３１２において、軸変数ｉが１
だけ加算されて、ステップ３０２に戻り、次の軸に関す
る慣性モーメント及び重力トルクの演算が繰り返し実行
される。又、軸変数ｉが最大値６になれば、ステップ３
００に戻り、軸変数ｉを１に初期設定して、１軸から演
算が繰り返し実行される。In step 312, the moment of inertia Di and the gravitational torque Ti are stored in the RAM as servo parameters.
It is updated and stored in 25 ITA areas. This value is output to the servo CPUs 22a to 22f in step 220 of FIG. Next, in step 310, it is determined whether or not the axis variable i has reached the maximum value 6. If not, in step 312, the axis variable i is set to 1
The calculation returns to step 302, and the calculation of the moment of inertia and the gravitational torque for the next axis is repeatedly executed. If the axis variable i reaches the maximum value 6, step 3
Returning to 00, the axis variable i is initialized to 1, and the calculation is repeatedly executed from one axis.

【００３７】この図７に示すプログラムは、上述したよ
うに、図４、図６に示すプログラムよりも優先度が１ラ
ンク低く設定されているので、図４、図６に示すプログ
ラムが実行されていない時に実行される。このようにし
て、補間演算手段（図６に示す手順を実行する手段）の
空き時間（２mS) において、図７に示す慣性モーメント
及び重力トルクの演算が実行される。従って、図７のプ
ログラムは、連続されて実行されるのではなく、８msec
の補間演算毎に中断され、中断された次の命令語から再
開される。このため、補間周期とは別の遅い周期で各軸
毎に慣性モーメント及び重力トルクが順次、最新の値に
更新されていくことになる。As shown in FIG. 7, the priority of the program shown in FIG. 7 is set lower than that of the program shown in FIGS. 4 and 6 by one rank, so that the program shown in FIGS. 4 and 6 is executed. Executed when not. In this way, the calculation of the moment of inertia and the gravitational torque shown in FIG. 7 is performed during the idle time (2 ms) of the interpolation calculation means (means for executing the procedure shown in FIG. 6). Therefore, the program of FIG. 7 is not executed continuously, but 8 msec.
Is interrupted every time the interpolation operation is performed, and is resumed from the interrupted next instruction word. For this reason, the moment of inertia and the gravitational torque are sequentially updated to the latest values for each axis in a slow cycle different from the interpolation cycle.

【００３８】次に、ｉ軸のサーボＣＰＵの動作手順を図
８のフローチャートに従って説明する。ステップ４００
において、エンコーダＥｉから現在の検出角度αi が読
み取られ、ステップ４０２において検出角度αi の時間
変化率から角速度βi 、角速度βi の時間変化率から角
加速度γi が演算される。次、ステップ４０４におい
て、ＲＡＭ２５の領域ＩＴＡに記憶されている最新の重
力トクルＴi と慣性モーメントＤi がＣＰＵ２０を介し
て受信され、ステップ４０６において、ＲＡＭ２５の領
域ＩＮＡに記憶されている角度指令値θi がＣＰＵ２０
を介して受信される。次に、ステップ４０８において、
角加速度γi と、慣性モーメントＤi とバネ定数Ｋc と
定数Ｋ2 に基づいて、次式により、角度指令値θi のフ
ィードフォワード補正が行われる。Next, the operation procedure of the i-axis servo CPU will be described with reference to the flowchart of FIG. Step 400
, The current detected angle αi is read from the encoder Ei, and in step 402, the angular velocity βi is calculated from the temporal change rate of the detected angle αi, and the angular acceleration γi is calculated from the temporal change rate of the angular velocity βi. Next, in step 404, the latest gravity torque Ti and moment of inertia Di stored in the area ITA of the RAM 25 are received via the CPU 20, and in step 406, the angle command value θi stored in the area INA of the RAM 25 is obtained. CPU 20
Is received via Next, in step 408,
Based on the angular acceleration γi, the moment of inertia Di, the spring constant Kc, and the constant K2, the feed-forward correction of the angle command value θi is performed by the following equation.

【００３９】[0039]

【数１１】θinew＝θi＋γiＤiＫ2／Ｋc[Equation 11] θinew = θi + γiDiK2 / Kc

【００４０】次に、ステップ４１０において、補正され
た角度指令値θinewの検出角度αiに対する角度偏差Δ
θi （＝θinew−αi ）が演算される。次に、ステップ
４１２において、角度偏差Δθi 、位置ループゲインＫ
p を用いて、次式により、角速度指令値ωi が演算され
る。Next, at step 410, the angle deviation Δ of the corrected angle command value θinew with respect to the detected angle αi
θi (= θinew-αi) is calculated. Next, in step 412, the angle deviation Δθi and the position loop gain K
Using p, the angular velocity command value ωi is calculated by the following equation.

【００４１】[0041]

【数１２】ωi ＝ＫpΔθiΩi = KpΔθi

【００４２】次に、ステップ４１４において、検出され
た角速度βi 、ゲインＫf を用いて、角速度のフィード
フォワード補正が次式により行われる。Next, in step 414, feedforward correction of the angular velocity is performed by the following equation using the detected angular velocity βi and gain Kf.

【数１３】ωinew＝ωi＋βiＫf 次に、ステップ４１６において、補正された角速度指令
値ωinewの検出された角速度βi に対する角速度偏差Δ
ωi （＝ωinew−βi ）が演算される。Ωinew = ωi + βiKf Next, at step 416, the angular velocity deviation Δ of the corrected angular velocity command value ωinew with respect to the detected angular velocity βi
ωi (= ωinew-βi) is calculated.

【００４３】次に、ステップ４１８において、電流指令
値Ｉi が、定数Ｋ、慣性モーメントＤi を用いて、次式
により、演算される。Next, in step 418, the current command value Ii is calculated by the following equation using the constant K and the moment of inertia Di.

【数１４】Ｉi ＝ＫＤi'Δωi 但し、Ｄi' は上記の慣性モーメントＤi にｉ軸のサー
ボモータのロータの慣性モーメントを加算した全慣性モ
ーメントである。このように電流指令値を求めるための
速度フィードバックループのゲインが全慣性モーメント
Ｄi'により補償されることになる。即ち、慣性モーメン
トが大きな姿勢では、追随性を向上させるために速度フ
ィードバックループのゲインが大きくなるように制御さ
れる。Ii = KDi'Δωi where Di 'is the total inertia moment obtained by adding the inertia moment of the rotor of the i-axis servo motor to the above inertia moment Di. Thus, the gain of the speed feedback loop for obtaining the current command value is compensated by the total inertia moment Di '. That is, in a posture having a large moment of inertia, control is performed so that the gain of the speed feedback loop is increased in order to improve followability.

【００４４】次に、ステップ４２０において、角加速度
γi と、全慣性モーメントＤi'とトルク定数Ｋtnと定数
Ｋa を用いて、次式により、電流指令値がフィードフォ
ワード補正される。Next, in step 420, the current command value is feedforward corrected by the following equation using the angular acceleration γi, the total moment of inertia Di ', the torque constant Ktn, and the constant Ka.

【数１５】Ｉinew＝Ｉi＋γiＫaＤi'／Ｋtn 次に、ステップ４２２において、最新の重力トルクＴi
と、ゲインＫT とを用いて、次式により、電流指令値の
重力トルクによる補正値が演算される。Iinew = Ii + γiKaDi ′ / Ktn Next, at step 422, the latest gravity torque Ti
And the gain KT, the correction value of the current command value by the gravitational torque is calculated by the following equation.

【数１６】Ｉinnew ＝Ｉinew＋ＴiＫT## EQU16 ## Iinnew = Iinew + TiKT

【００４５】次に、ステップ４２４において、最終的に
演算された電流指令値Ｉinnew に基づいて、ｉ軸のサー
ボモータに対する電流制御が実行され、このサーボモー
タは慣性モーメント及び重力トルクの補償がされた状態
で角度指令値θi に向けて位置決めされる。Next, in step 424, the current control for the i-axis servomotor is executed based on the finally calculated current command value Iinnew, and the servomotor is compensated for the moment of inertia and the gravitational torque. In this state, positioning is performed toward the angle command value θi.

【００４６】以上の処理が微小時間間隔で繰り返し実行
されることにより、図３に示す回路機能と等価な処理が
実行される。尚、上記実施例では、慣性行列Ｄi の対角
成分だけで慣性モーメントの補償を行ったが、他の軸と
の相互作用を表した非対角成分を含めた補償を行うよう
にしても良い。By repeating the above processing at minute time intervals, processing equivalent to the circuit function shown in FIG. 3 is performed. In the above embodiment, the moment of inertia is compensated only by the diagonal components of the inertia matrix Di. However, the compensation may be performed by including the non-diagonal components representing the interaction with other axes. .

【００４７】尚、上記実施例では、床置きタイプのロボ
ットの場合について述べたが、天吊りタイプのロボット
の場合には、図９の配置関係のバランサにおいて、数
９、数１０式に代えて、次の数１７、数１８式となる。In the above embodiment, the case of the floor-standing type robot has been described. However, in the case of the ceiling-mounted type robot, the formula (9) or (10) is used instead of the formulas (9) and (10) in the arrangement relational balancer shown in FIG. , And the following equations 17 and 18 are obtained.

【数１７】ｆ＝δ×ｋ＋ｆ0F = δ × k + f0

【数１８】ｔ2＝ｒ×ｆ×sinβ×ｎT2 = r × f × sinβ × n

【００４８】又、上記実施例では、コイルスプリングの
バランサを用いているが、図１０に示すようなエアーバ
ランサを用いても良い。その場合には、補償トルクの演
算式は次のようになる。 (1) エアーバランサーの床置きタイプの場合In the above embodiment, a balancer of a coil spring is used, but an air balancer as shown in FIG. 10 may be used. In that case, the formula for calculating the compensation torque is as follows. (1) In the case of an air balancer on the floor

【数１９】ｌ＝（ｒ2＋ｈ2−２ｒ×ｈ×cosα) 1/2[Mathematical formula-see original document] l = (r2 + h2-2r * h * cos [alpha]) 1/2

【数２０】δ＝ｌ−（ｒ−ｈ）Δ = l− (r−h)

【数２１】β＝tan-1｛ｈ sinα／（ｒ−ｈ cosα) ｝Β = tan-1 {h sinα / (r−h cosα)}

【数２２】Ａ＝（π／４) (Ｄ2−ｄ2）A = (π / 4) (D2−d2)

【数２３】Ｐ2×｛Ｖ2＋ｎ×Ａ×（ｓ−δ）｝＝Ｐ1×
（Ｖ2＋ｎ×Ａ×ｓ）[Equation 23] P2 × {V2 + n × A × (s−δ)} = P1 ×
(V2 + n × A × s)

【数２４】Ｐ2＝｛Ｐ1×（Ｖ2＋ｎ×Ａ×ｓ）｝／｛Ｖ2
＋ｎ×Ａ×（ｓ−δ）｝P2 = {P1 × (V2 + n × A × s)} / {V2
+ N × A × (s−δ)｝

【数２５】ｆ＝Ａ×Ｐ2F = A × P2

【数２６】ｔ2＝ｒ×ｆ×sinβ×ｎ[Formula 26] t2 = r × f × sinβ × n

【００４９】ただし、 ｓ：シリンダーの全ストローク (m) Ｄ：シリンダーの直径 (m) ｄ：ピストンロッドの直径 (m) Ａ：シリンダーの有効断面積 (m2) Ｖ2 ：サージタンクの体積 (m3) Ｐ1 ：α＝０の時の圧力 (Pa) Ｐ2 ：α＝ジョイント角θ2の時の圧力 (Pa) ｎ：バランサ数Where s: total stroke of cylinder (m) D: diameter of cylinder (m) d: diameter of piston rod (m) A: effective area of cylinder (m2) V2: volume of surge tank (m3) P1: Pressure at α = 0 (Pa) P2: α = Pressure at joint angle θ2 (Pa) n: Number of balancers

【００５０】(2) エアーバランサーの点吊りタイプの場
合(2) In the case of a point suspension type air balancer

【数２７】ｌ＝（ｒ2＋ｈ2−２ｒ×ｈ×cosα)1/2[Mathematical formula-see original document] l = (r2 + h2-2r * h * cos [alpha]) 1/2

【数２８】δ＝ｌ−（ｒ−ｈ）Δ = l− (r−h)

【数２９】β＝tan-1｛ｈ sinα／（ｒ−ｈ cosα) ｝Β = tan-1 {h sinα / (r−h cosα)}

【数３０】Ａ＝（π／４）Ｄ2A = (π / 4) D2

【数３１】ｌL＝（ｒ2＋ｈ2−２ｒ×ｈ×cosαL)1/2## EQU31 ## lL = (r2 + h2-2r.times.h.times.cos.alpha.L) 1/2

【数３２】δL＝ｌL−（ｒ−ｈ）## EQU32 ## δL = 1L− (r−h)

【数３３】Ｐ2×（Ｖ2＋ｎ×Ａ×δ）＝Ｐ1×（Ｖ2＋ｎ
×Ａ×δL）P2 × (V2 + n × A × δ) = P1 × (V2 + n
× A × δL)

【数３４】Ｐ2＝｛Ｐ1×（Ｖ2＋ｎ×Ａ×δL）｝／（Ｖ
2＋ｎ×Ａ×δ）P2 = {P1 × (V2 + n × A × δL)} / (V
2 + n × A × δ)

【数３５】ｆ＝−Ａ×Ｐ2F = −A × P2

【数３６】ｔ2＝ｒ×ｆ×sinβ×ｎT2 = r × f × sinβ × n

【００５１】ただし、 ｌL：α＝αLの時のバランサーの長さ (m) αL：ジョイント２のリミット角 (rad) δL：α＝αLの時のバランサーのたわみ (m) Ｐ1：α＝αLの時の圧力 (Pa)Where lL: length of balancer when α = αL (m) αL: limit angle of joint 2 (rad) δL: deflection of balancer when α = αL (m) P1: α = αL Time pressure (Pa)

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

【図１】本発明の具体的な実施例装置で使用されるロボ
ットを示した構成図。FIG. 1 is a configuration diagram showing a robot used in a specific embodiment device of the present invention.

【図２】ロボットの制御装置の構成を示したブロック
図。FIG. 2 is a block diagram showing a configuration of a control device of the robot.

【図３】図２におけるサーボＣＰＵの処理を示したブロ
ック図。FIG. 3 is a block diagram showing processing of a servo CPU in FIG. 2;

【図４】ロボットの位置及び姿勢制御のための主プログ
ラムを示したフローチャート。FIG. 4 is a flowchart showing a main program for controlling the position and posture of the robot.

【図５】ロボットの位置及び姿勢制御のための動作プロ
グラムを示した説明図。FIG. 5 is an explanatory diagram showing an operation program for controlling the position and posture of the robot.

【図６】補間演算の手順を示したフローチャート。FIG. 6 is a flowchart showing a procedure of an interpolation calculation.

【図７】慣性モーメント／重力トルクの演算手順を示し
たフローチャート。FIG. 7 is a flowchart showing a calculation procedure of a moment of inertia / gravity torque.

【図８】サーボＣＰＵによる角度制御の処理手順を示し
たフローチャート。FIG. 8 is a flowchart showing a procedure of an angle control process by a servo CPU.

【図９】スプリングバランサの取付け状態における幾何
学的構成を示した説明図。FIG. 9 is an explanatory diagram showing a geometric configuration in a mounted state of a spring balancer.

【図１０】エアーバランサの取付け状態における幾何学
的構成を示した説明図。FIG. 10 is an explanatory diagram showing a geometric configuration in an attached state of the air balancer.

【符号の説明】[Explanation of symbols]

１０…ロボット １１…バランサ １５…アッパーアーム １２０…バランサ １８…スイベルリスト １８ａ…フランジ １９…ハンド ２０…ＣＰＵ（補間演算手段、モーメント／トルク演算
手段、補償トルク演算手段、トルク合成手段） ２２ａ〜２２ｆ…サーボＣＰＵ（角度制御手段、第１補
正手段、第２補正手段） ２５…ＲＡＭ（教示データ記憶手段） Ｅ１〜Ｅ６…エンコーダ（角度検出手段） ステップ２００〜２２２…補間演算手段 ステップ３００〜３０６…モーメント／トルク演算手段 ステップ３０８…補償トルク演算手段 ステップ３１０…トルク合成手段 ステップ４００〜４２４…角度制御手段DESCRIPTION OF SYMBOLS 10 ... Robot 11 ... Balancer 15 ... Upper arm 120 ... Balancer 18 ... Swivel list 18a ... Flange 19 ... Hand 20 ... CPU (Interpolation calculating means, moment / torque calculating means, compensation torque calculating means, torque combining means) 22a-22f ... Servo CPU (angle control means, first correction means, second correction means) 25 ... RAM (teaching data storage means) E1 to E6 ... encoder (angle detection means) Step 200 to 222 ... interpolation calculation means Step 300 to 306 ... moment / Torque calculating means Step 308: Compensating torque calculating means Step 310: Torque combining means Steps 400 to 424: Angle control means

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 昭60−3714（ＪＰ，Ａ) 特開 昭61−173885（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05B 19/18 - 19/46 B25J 3/00 - 3/04 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06 ────────────────────────────────────────────────── ─── Continuation of the front page (56) References JP-A-60-3714 (JP, A) JP-A-61-173885 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) G05B 19/18-19/46 B25J 3/00-3/04 B25J 9/10-9/22 B25J 13/00-13/08 B25J 19/02-19/06

Claims (2)

(57)【特許請求の範囲】(57) [Claims] 【請求項１】軸にかかる重力トルクを減少させるための
バランサを有したロボットの制御装置であって、位置フ
ィードバックループ、速度フィードバックループ及び電
流フィードバックループを有し、前記ロボットの工具を
指令された位置及び姿勢に追随させる制御装置におい
て、ロ ボットの各軸の回転角を検出する角度検出手段と、 前記角度検出手段により検出された各軸の回転角とロボ
ットの各リンクの長さ及び質量とから慣性モーメント及
び重力トルクを演算するモーメント／トルク演算手段
と、 前記角度検出手段により検出された各軸の回転角、及
び、バランサの構造に基づいた定数から前記バランサの
取り付けられている軸の回りの前記バランサによる補償
トルクを演算する補償トルク演算手段と、 前記モーメント／トルク演算手段により演算された重力
トルクと前記補償トルク演算手段により演算された補償
トルクとを合成するトルク合成手段と、 前記モーメント／トルク演算手段により演算された慣性
モーメントに基づいて前記速度フィードバックループの
ゲインを変化させる第１補正手段と、 前記トルク合成手段により演算された合成トルクに基づ
いて前記電流フィードバックループへの指令値を加算補
正する第２補正手段とから成るロボットの制御装置。1. A control device for a robot having a balancer for reducing a gravitational torque applied to an axis, comprising a position feedback loop, a speed feedback loop, and a current feedback loop, wherein a tool of the robot is provided.
A control apparatus for following the directive position and orientation, and angle detecting means for detecting a rotation angle of each axis of the robot, the length of each link of the rotation angle and the robot axes detected by said angle detecting means and and moment / torque calculating means for computation of the mass Toka et moment of inertia and gravity torque, the rotation angle of each axis detected by the angle detection means, and, before Symbol balancer constant based on the structure of the balancer a compensation torque calculating means for calculating a compensation <br/> torque by the balancer around on the installed shaft, compensation calculated by the compensation torque calculation means and the calculated gravity torque by the moment / torque calculation means Torque synthesizing means for synthesizing a torque, and the speed feed based on the moment of inertia calculated by the moment / torque calculating means. A first correction means for varying the gain of Kkurupu, the controller of the robot and a second correction means for adding correcting the command value for the current feedback loop based on the calculated combined torque by the torque combining means. 【請求項２】請求項１のロボットの制御装置において、 前記モーメント／トルク演算手段及び補償トルク演算手
段を、前記工具が指令された位置への移動経路を決定す
るために周期的に実行される補間点演算の間の空時間を
用いて実行するロボットの制御装置。 2. The robot controller according to claim 1, wherein said moment / torque calculating means and a compensation torque calculating means are provided.
Step to determine the path of travel of the tool to a commanded position.
Time between interpolation point calculations that are performed periodically to
A control device for the robot to be executed using.