JPH07120212B2 - Industrial robot controller - Google Patents
Industrial robot controllerInfo
- Publication number
- JPH07120212B2 JPH07120212B2 JP60097525A JP9752585A JPH07120212B2 JP H07120212 B2 JPH07120212 B2 JP H07120212B2 JP 60097525 A JP60097525 A JP 60097525A JP 9752585 A JP9752585 A JP 9752585A JP H07120212 B2 JPH07120212 B2 JP H07120212B2
- Authority
- JP
- Japan
- Prior art keywords
- electric motor
- output
- arm
- value
- rotation angle
- 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.)
- Expired - Fee Related
Links
Description
【発明の詳細な説明】 産業上の利用分野 本発明は、産業用ロボツトのアーム先端の振動を抑制
し、同時にアーム先端の位置決め精度の向上を図ること
ができる産業用ロボツトの制御装置に関する。Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for an industrial robot capable of suppressing vibration of an arm end of an industrial robot and at the same time improving positioning accuracy of the arm end.
背景技術 産業用ロボツトの制御方法は、ロボツトのたとえばアー
ムを構成する各関節軸毎のサーボ系において、電動機に
備えられ、電動機の出力軸の回転角度を検出するエンコ
ーダと称される回転角度検出器の出力と、電動機に備え
られるタコジエネレータと称される回転速度検出器の出
力とをフイードバツクする方法が通例である。BACKGROUND ART A method for controlling an industrial robot is a rotation angle detector called an encoder that is provided in an electric motor and detects a rotation angle of an output shaft of the electric motor in a servo system for each joint axis that constitutes, for example, an arm of the robot. Is generally fed back to the output of a rotation speed detector called a tachogenerator provided in the electric motor.
ここでスポツト溶接用ロボツトなどの高負荷重量の電動
ロボツトにおいては、ロボツトアームの慣性モーメント
が大きく、減速機、トルク伝達機構およびアームなどの
ばね要素により、アームの振動に関する固有周波数が低
下する。したがつてアーム先端に低周期で振幅の大きな
振動が発生しやすくなり、またアーム先端の位置決め精
度が低下するなどの問題点がある。Here, in an electric robot with a heavy load such as a spot welding robot, the inertia moment of the robot arm is large, and the natural frequency related to the vibration of the arm is lowered by the spring elements such as the speed reducer, the torque transmission mechanism and the arm. Therefore, there is a problem that vibration with a large period and large amplitude is easily generated at the arm tip, and the positioning accuracy of the arm tip is lowered.
このような問題点に関して、上記先行技術の制御方法に
おける回転角度検出器の出力と、回転速度検出器の出力
との２つの物理量のみのフイードバツクでは、アーム先
端の前記振動や位置誤差を補償することができなかつ
た。With respect to such a problem, in the feedback of only two physical quantities, that is, the output of the rotation angle detector and the output of the rotation speed detector in the control method of the above-mentioned prior art, it is necessary to compensate the vibration and the position error of the arm tip. I couldn't do it.
このような問題点を解決する他の先行技術として、メカ
ニカルダンパを用いることが試みられているけれども、
アーム先端の振動を防止するに効果的な設置場所が得ら
れ難く、また高コストになるなどの問題点があつた。Although it has been attempted to use a mechanical damper as another prior art for solving such a problem,
There are problems that it is difficult to obtain an effective installation location for preventing vibration of the arm tip and the cost becomes high.
また上述のような問題点を解決するためのさらに他の先
行技術として、いわゆるノツチフイルタを使用して、ロ
ボツトアームの振動を抑制することが行なわれている。
一方、ロボツトの各種特性は、ロボツトの姿勢の変動に
よつて大きく変化するため、ロボツトのあらゆる姿勢に
おいて、振動抑制効果を得ることが困難である。Further, as still another prior art for solving the above problems, a so-called notch filter is used to suppress the vibration of the robot arm.
On the other hand, since various characteristics of the robot change greatly depending on the change of the robot posture, it is difficult to obtain the vibration suppressing effect in all the postures of the robot.
また加速度センサや歪ゲージなどを用いてアーム先端の
加速度を計測し、これらの出力をフイードバツクするこ
とによつて振動を止める制御方法も採用されているけれ
ども、センサの設置場所やフイルタリングを最適に設計
することが困難であるなどの問題点があつた。A control method is also adopted to measure the acceleration of the arm tip using an acceleration sensor or strain gauge and stop the vibration by feeding back these outputs.However, the sensor installation location and filtering are optimized. There were problems such as difficulty in designing.
発明が解決しようとする問題点 したがつて本発明は、上述のような各先行技術における
諸問題を解決し、電動機の回転角度、回転速度および電
動機電流の指令値などの既に測定されている物理量のみ
から、アーム先端の位置および速度を推定し、これを最
適にフイードバツクすることによつて、アーム先端の振
動と位置誤差とを補償することができる産業用ロボツト
の制御装置を提供することを目的とする。SUMMARY OF THE INVENTION Therefore, the present invention solves the problems in each of the prior arts as described above, and the already measured physical quantities such as the rotation angle of the electric motor, the rotational speed, and the command value of the electric motor current. The purpose of the present invention is to provide a controller for an industrial robot capable of compensating for the vibration and position error of the arm tip by estimating the position and velocity of the arm tip from only the above and optimally feeding back this. And
問題点を解決するための手段 本発明は、電動機１の軸に、回転速度検出器４および回
転角度検出器３が結合されるとともに減速歯車５が結合
され、該減速歯車５の出力軸がトルク伝達機構６を介し
てアーム７を駆動するような構造を有する産業用ロボツ
トと、 電動機１の前記軸の回転角度の指令値θｒを発生する手
段と、 前記回転角度の指令値θｒから回転角度検出器３によつ
て検出される電動機回転変位θ１の出力を減算する第１
減算器S1と、 電動機電流指令値u1を導出する第２減算器S2と、 前記電動機１の回転子、減速歯車５、トルク伝達機構６
およびアーム７によつて構成される軸系をばね・質量か
らなる系と仮定し、電動機電流指令値u1、電動機回転変
位θ１、電動機回転速度１、および外部トルクu2から
前記ばね・質量系のねじれ角度θ３の推定値 と、そのねじれ角度θ３の時間微分値３の推定値 とを求める状態推定器11と、 前記ねじれ角θ３の推定値 と前記時間微分値３の推定値 とを加算する加算器S5とを含み、 第２減算器S2は、第１減算器S1の出力から、回転速度検
出器４の出力１と加算器S5の出力とを減算して前記電
動機電流指令値u1を導出して電動機１に与えることを特
徴とする産業用ロボツトの制御装置である。Means for Solving the Problems According to the present invention, a rotation speed detector 4 and a rotation angle detector 3 are coupled to a shaft of an electric motor 1 and a reduction gear 5 is coupled thereto, and an output shaft of the reduction gear 5 has a torque. An industrial robot having a structure for driving the arm 7 via the transmission mechanism 6, a unit for generating a command value θr of a rotation angle of the shaft of the electric motor 1, and a rotation angle detection from the command value θr of the rotation angle. For subtracting the output of the motor rotational displacement θ1 detected by the device 3
Subtractor S1, second subtractor S2 for deriving electric motor current command value u1, rotor of electric motor 1, reduction gear 5, torque transmission mechanism 6
Assuming that the shaft system configured by the arm 7 and the arm 7 is a system including a spring and a mass, the motor current command value u1, the motor rotational displacement θ1, the motor rotation speed 1, and the external torque u2 are used to twist the spring and mass system. Estimated value of angle θ3 And the estimated value of the time derivative 3 of the twist angle θ3 And a state estimator 11 for obtaining and an estimated value of the twist angle θ3. And the estimated value of the time differential value 3 The second subtractor S2 subtracts the output 1 of the rotation speed detector 4 and the output of the adder S5 from the output of the first subtractor S1 to add the This is a control device for an industrial robot characterized in that a value u1 is derived and given to the electric motor 1.
作 用 本発明に従えば、電動機軸の一端に回転速度検出器およ
び回転角度検出器が結合され、他の一端に減速歯車が結
合され、該減速歯車の出力軸がトルク伝達機構を介して
アームを駆動するような構造を有する産業用ロボツトを
準備する。該ロボツトにおける電動機の回転子、減速歯
車、トルク伝達機構およびアームによつて構成される軸
系をばね・質量からなる系と仮定し、電動機電流指令
値、電動機回転変位θ１および電動機回転速度１から
ばね・質量系のねじれ角度θ３の推定値 とねじれ角度θ３の時間微分値 とを状態推定器11によつて求めて加算器S5によつて加算
し、これによつてアーム回転変位θ２およびアーム回転
速度２に対応する値を測定して求め、このようにして
電動機回転変位θ１、電動機回転速度１、アーム回転
変位θ２およびアーム回転速度２の４個の状態量を全
てフイードバツクして、優れた制御性能を実現すること
ができるようになる。Operation According to the present invention, the rotation speed detector and the rotation angle detector are coupled to one end of the motor shaft, the reduction gear is coupled to the other end, and the output shaft of the reduction gear is connected to the arm via the torque transmission mechanism. An industrial robot having a structure for driving a motor is prepared. Assuming that the shaft system constituted by the rotor of the electric motor, the reduction gear, the torque transmission mechanism and the arm in the robot is a system consisting of springs and masses, from the electric motor current command value, the electric motor rotational displacement θ1 and the electric motor rotational speed 1. Estimated value of torsion angle θ3 of spring / mass system And the time derivative of the twist angle θ3 Are obtained by the state estimator 11 and added by the adder S5, and thereby the values corresponding to the arm rotational displacement θ2 and the arm rotational speed 2 are measured and obtained. It is possible to realize excellent control performance by feeding back all four state quantities of θ1, electric motor rotation speed 1, arm rotation displacement θ2, and arm rotation speed 2.
実施例 第１図は本発明の一実施例の構成を説明する斜視図であ
る。電動機１は、駆動回路２の出力であるたとえば電流
によつて駆動される。電動機１の一端には、回転角度検
出器３が結合され、出力軸1aの回転角度の検出を行な
う。回転角度検出器３と同様に、電動機１に回転速度検
出器４が結合されており、出力軸1aの回転速度の検出を
行なう。Embodiment FIG. 1 is a perspective view for explaining the structure of an embodiment of the present invention. The electric motor 1 is driven by the output of the drive circuit 2, for example, an electric current. A rotation angle detector 3 is coupled to one end of the electric motor 1 to detect the rotation angle of the output shaft 1a. Similar to the rotation angle detector 3, a rotation speed detector 4 is coupled to the electric motor 1 to detect the rotation speed of the output shaft 1a.
電動機１の出力軸1aには、減速機５を構成する歯車5aが
取付けられる。一方、減速機５の出力軸には、前記歯車
5aと組合わされる歯車5bが取付けられる。またこの出力
軸にはトルク伝達機構６が結合され、トルク伝達機構６
の他端には、ロボツトのアーム７が取付けられる。電動
機１の回転トルクは、減速機５およびトルク伝達機構６
を介して、ロボツトのアーム７に伝達される。この電動
機１は、制御装置８によつて制御される。第１図の構成
において、制御装置８以外の構成を以下に制御対象と称
する。To the output shaft 1a of the electric motor 1, a gear 5a that constitutes the speed reducer 5 is attached. On the other hand, on the output shaft of the speed reducer 5, the gear
A gear 5b associated with 5a is mounted. Further, a torque transmission mechanism 6 is coupled to this output shaft, and the torque transmission mechanism 6
A robot arm 7 is attached to the other end of the robot. The rotation torque of the electric motor 1 is calculated by the reduction gear 5 and the torque transmission mechanism 6.
Is transmitted to the arm 7 of the robot via. The electric motor 1 is controlled by the control device 8. In the configuration of FIG. 1, a configuration other than the control device 8 will be referred to as a controlled object hereinafter.
第２図は第１図示の構成の機構的に等価な構成を示す斜
視図である。第１図および第２図を参照して、減速機
５、トルク伝達機構６およびアーム７の有するばね特性
による捩れおよび撓みなどに着目すれば、第１図示の制
御対象は第２図示のように、電動機１の出力軸1aに結合
されたばね９と、ばね９の先端に結合された質量10とか
らなる構成と等価である。FIG. 2 is a perspective view showing a mechanically equivalent structure of the structure shown in FIG. Referring to FIG. 1 and FIG. 2, focusing on the twisting and bending due to the spring characteristics of the speed reducer 5, the torque transmission mechanism 6, and the arm 7, the controlled object shown in FIG. 1 is as shown in FIG. It is equivalent to a configuration including a spring 9 coupled to the output shaft 1a of the electric motor 1 and a mass 10 coupled to the tip of the spring 9.
第３図は第１図の制御対象と等価なブロツク図である。
電動機１の出力軸1aの回転角度をθ１、アーム７の回転
角度をθ２とすると、出力軸1aの回転速度は１または
アーム７の回転速度は２である。また電動機１の駆動
回路２に対する電流の指令値をu1、重力などのアーム７
に加えられる外部トルクをu2とする。FIG. 3 is a block diagram equivalent to the controlled object of FIG.
When the rotation angle of the output shaft 1a of the electric motor 1 is θ1 and the rotation angle of the arm 7 is θ2, the rotation speed of the output shaft 1a is 1 or the rotation speed of the arm 7 is 2. Further, the command value of the current to the drive circuit 2 of the electric motor 1 is u1, the arm 7 such as gravity is used.
The external torque applied to is u2.
ここで第２図示の構成を、以下にモデルと称する。この
モデルの動特性は、下式のような微分方程式で記述され
る。Here, the configuration shown in the second illustration will be referred to as a model hereinafter. The dynamic characteristics of this model are described by the differential equations shown below.
＝Ａ・ｘ＋Ｂ・ｕ ……（１） ｘ＝（1,θ1,2,θ２）Ｔ、ｕ＝（u1,u2）Ｔ また、 である。ここで前記ｕの右辺において、添字Ｔは行列
（u1,u2）の転置行列を示す。= A ・ x + B ・ u (1) x = (1, θ1,2, θ2) T , u = (u1, u2) T Is. Here, on the right side of the u, the subscript T indicates the transposed matrix of the matrix (u1, u2).
また第３図示の各ブロツク内の記号の意味は下記のとお
りである。The meanings of the symbols in each block shown in the third illustration are as follows.
KA;電流アンプゲイン Re（＝Ｒ＋KA・KAF）；電流ループ相当ゲイン KT;電動機トルク定数 JM;電機子慣性モーメント RG;ギヤ比 KC;ばね定数 DM;電動機摩擦係数 D2;二次側摩擦係数 D1;ばね系摩擦係数 JL;二次側慣性モーメント KE;逆起電力定数 S;ラプラス変換子 R;電機子抵抗 KAF;電流フイードバツクゲイン 第３図示の等価回路または前記第１式に前述したような
電動機１の駆動回路２に対する電流の指令値u1および外
部トルクu2を入力すれば、θ1,1,θ2,２を推定する
ことができる。一方後述されるように本発明において
は、電動機１の回転角度θ１とアーム７の回転角度θ２
のずれ、およびその微分値の推定が必要である。したが
つて前記モデルにおけるばね・出力系のねじれ角度をθ
３とし、その微分値を３とすると、下式が成立する。K A ; Current amplifier gain Re (= R + K A · K AF ); Current loop equivalent gain K T ; Motor torque constant J M ; Armor moment of inertia R G ; Gear ratio K C ; Spring constant D M ; Motor friction coefficient D 2 ; Secondary side friction coefficient D 1 ; Spring system friction coefficient J L ; Secondary side moment of inertia K E ; Back electromotive force constant S; Laplace converter R; Armor resistance K AF ; Current feedback gain 3rd drawing If the command value u1 of the current to the drive circuit 2 of the electric motor 1 and the external torque u2 as described above are input to the equivalent circuit of (1) or the first expression, θ1,1, θ2,2 can be estimated. On the other hand, as will be described later, in the present invention, the rotation angle θ1 of the electric motor 1 and the rotation angle θ2 of the arm 7
It is necessary to estimate the deviation and its derivative. Therefore, the torsion angle of the spring and output system in the model is θ
If the differential value is 3 and the differential value is 3, the following formula is established.
前記第２式および第３式を第１式に代入することによ
り、下式が得られる。 By substituting the second equation and the third equation into the first equation, the following equation is obtained.
＝（1,θ1,3,θ３）Ｔ 12＝RG・a12＋a14 13 ＝−a13 14 ＝RG・a12 34＝a12−RG・a32 11 ＝b11 32＝−b32 である。 = (1, θ1,3, θ3) T 12 = RG・ a 12 ＋ a 14 13 ＝ －a 13 14 ＝ R G・ a 12 34 = a 12 -R G / a 32 11 = b 11 32 = −b 32 .
上記第４式に、電動機１の駆動回路２に対する電流の指
令値u1および外部トルクu2を入力すれば、θ1,1,θ3,
３を推定することができる。一方、１は回転速度検
出器４によつて、またθ１は回転角度検出器３によつ
て、それぞれ計測されることが可能であり、したがつて
推定動作を行なう必要はない。そのため前記第４式を下
記の第５式および第６式のように変形して第５式に、u1
として電動機１の駆動回路２に対する電流の指令値、u2
として外部トルクの計算値、θ１として回転角度検出器
３の出力、１として回転速度検出器４の出力をそれぞ
れ入力することにより、簡便な操作によつて、前記モデ
ルにおけるばね・質量系のねじれ角度θ３とその微分値
θ３とを推定することができる。If the command value u1 of the current to the drive circuit 2 of the electric motor 1 and the external torque u2 are input to the above formula 4, θ1, 1, θ3,
3 can be estimated. On the other hand, 1 can be measured by the rotation speed detector 4 and θ1 can be measured by the rotation angle detector 3, so that it is not necessary to perform the estimation operation. Therefore, the fourth equation is modified into the following fifth and sixth equations to obtain the fifth equation, u1
, The command value of the current for the drive circuit 2 of the electric motor 1, u2
By inputting the calculated value of the external torque as θ1, the output of the rotation angle detector 3 as θ1, and the output of the rotation speed detector 4 as θ1, respectively, the twist angle of the spring / mass system in the model can be calculated by a simple operation. It is possible to estimate θ3 and its differential value θ3.
＝A11・ｚ＋A12・ｙ＋B1・ｕ ……（５） ＝A21・ｚ＋A22・ｙ＋B2・ｕ ……（６） ｚ＝（3,θ３）Ｔ、ｙ＝（，θ１）Ｔ 上述したような推定動作に引続き、さらに上記第５式を
用いて推定を行なう場合において、計算上誤差が生じた
とき、その誤差を自律的に任意の速度で修正する機能を
持たせるために、下式を用いて状態推定器の状態変数ｚ
をｚ′に変更する。＝ A 11・ z + A 12・ y + B 1・ u …… (5) ＝ A 21・ z + A 22・ y + B 2・ u …… (6) z = (3, θ3) T , y = (, θ1) T Subsequent to the above-described estimation operation, when an error is caused in calculation when the estimation is performed using the above-mentioned formula 5, the error is calculated. In order to have the function of autonomously correcting at an arbitrary speed, the state variable z of the state estimator is
To z '.
ｚ′＝ｚ＋Ｋ・ｙ ……（７） 上記第７式をｚについて解いて第５式に代入すれば、後
述される状態推定器の状態方程式は、下記の第８式およ
び第９式のように変更される。z ′ = z + K · y (7) If the above-mentioned 7th formula is solved about z and it substitutes in the 5th formula, the state equation of a state estimator mentioned below will be changed like the following 8th and 9th formulas.
′＝Ｆ・ｚ′＋Ｇ・ｙ＋Ｗ・ｕ ……（８） ｚ＝ｚ′−Ｊ・ｙ ……（９） ただし、Ｆ＝A11＋Ｋ・A21 Ｇ＝A12＋Ｋ・A22−A11・Ｋ ＋Ｋ・A21・Ｋ Ｗ＝B1＋Ｋ・B2 Ｊ＝−Ｋ 上述したような推定動作のための演算を行なうことがで
きるように状態推定器を構成すれば、後述されるように
２つの積分器によつて前述の制御対象の状態量を推定す
ることができる。′ = F · z ′ + G · y + W · u (8) z = z′-J · y (9) However, F = A 11 + K · A 21 G = A 12 + K · A 22 −A 11 by configuring the · K + K · a 21 · K W = B 1 + K · B 2 J = -K state estimator to be able to perform the calculation for the estimation operation described above, as will be described later The state quantity of the controlled object can be estimated by the two integrators.
最小次数オブザーバ理論によれば、前記行列A11＋Ｋ・A
21の固有値を、行列Ｋの選び方によつて所望の値に設定
することができる。さらに本実施例においては、回転角
度検出器３の出力θ１の係数が零になるように、行列Ｋ
を選ぶことができ、これらを満足するような行列Ｋの要
素K11、K12、K21、K22は下記の第10式のように算出する
ことができる。According to the minimum order observer theory, the matrix A 11 + K · A
The 21 eigenvalues can be set to desired values depending on how the matrix K is selected. Further, in this embodiment, the matrix K is set so that the coefficient of the output θ1 of the rotation angle detector 3 becomes zero.
The elements K 11 , K 12 , K 21 and K 22 of the matrix K satisfying these can be calculated by the following formula 10.
K21＝K22＝０ ただし、λ1,λ２は、行列A11＋Ｋ・A21の固有値であ
る。 K 21 = K 22 = 0 However, λ1 and λ2 are eigenvalues of the matrix A 11 + K · A 21 .
本実施例における上述したような構成と動作を有する状
態推定器、すなわち前記第８式および第９式に第10式を
代入することによつて得られる下記の第11式および第12
式の状態方程式によつて記述される状態推定器に、回転
速度検出器４の出力１と電動機１の駆動回路２に対す
る電流の指令値u1と外部トルクの計算値u2のみを入力す
る。この操作によつて、前記第２図示のモデルにおける
ばね・質量系のねじれ角度θ３とその微分値３と、こ
れらの各推定値 とを得ることができる。A state estimator having the above-described configuration and operation in the present embodiment, that is, the following formulas 11 and 12 obtained by substituting formula 10 for formulas 8 and 9 above.
Only the output 1 of the rotation speed detector 4, the command value u1 of the current to the drive circuit 2 of the electric motor 1 and the calculated value u2 of the external torque are input to the state estimator described by the state equation of the equation. By this operation, the torsion angle θ3 of the spring / mass system in the model shown in the second illustration and its derivative value 3 and their respective estimated values And you can get
第４図は上記第11式および第12式の状態方程式で記述さ
れる状態推定器11を、計算機のプログラムまたは電気回
路で実現する場合の一実施例のブロツク図である。本実
施例の状態推定器11には、２つの積分器I1,I2が用いら
れており、電動機１（第１図参照）の駆動回路２に対す
る電流の指令値u1に係合器W11を乗じた演算結果と、外
部トルク入力u2に係合器W12を乗じた演算結果と、回転
速度検出器４の出力１に係数器G11を乗じた演算結果
と、積分器I1の出力z1に係数器F11を乗じた演算結果
と、積分器I2の出力z2に係数器F11を乗じた演算結果と
を加算器S1によつて加算する。この加算器S1の出力が、
積分器I1への入力となる。また電動機１（第１図参照）
の駆動回路２に対する電流の指令値u1に係数器W21を乗
じた演算結果と、外部トルク入力u2に係数器W22を乗じ
た演算結結果と、回転速度検出器４の出力１に係数器
G21を乗じた演算結果と、積分器I1の出力z1に係数器F21
を乗じた演算結果と、積分器I2の出力z2に係数器F22を
乗じた演算結果とを加算器S2によつて加算する。この加
算器S2の出力が積分器I2への入力となる。 FIG. 4 is a block diagram of an embodiment in which the state estimator 11 described by the state equations 11 and 12 is realized by a computer program or an electric circuit. Two integrators I1 and I2 are used for the state estimator 11 of the present embodiment, and the command value u1 of the current to the drive circuit 2 of the electric motor 1 (see FIG. 1) is multiplied by the engaging device W11. The calculation result, the calculation result of multiplying the external torque input u2 by the engaging device W12, the calculation result of multiplying the output 1 of the rotation speed detector 4 by the coefficient unit G11, and the output z1 of the integrator I1 by the coefficient unit F11. The multiplication result and the calculation result obtained by multiplying the output z2 of the integrator I2 by the coefficient unit F11 are added by the adder S1. The output of this adder S1 is
It is an input to the integrator I1. Also, electric motor 1 (see FIG. 1)
Of the electric current command value u1 for the driving circuit 2 of FIG. 3 and the calculation result of multiplying the external torque input u2 by the coefficient unit W22, and the output 1 of the rotation speed detector 4 by the coefficient unit
The calculation result obtained by multiplying G21 and the output z1 of the integrator I1 to the coefficient unit F21
The calculation result obtained by multiplying the output z2 of the integrator I2 by the coefficient unit F22 is added by the adder S2. The output of this adder S2 becomes the input to the integrator I2.
次に回転速度検出器４の出力１に係数器J31を乗じた
演算結果と、積分器I1の出力z1を加算器S3によつて加算
する。この加算器S3の出力を、前述したモデルにおける
ばね・質量系のねじれ角度θ３の微分値の推定値 とする。また回転速度検出器４の出力１に係数器J41
を乗じた演算結果と、積分器I2の出力z2を加算器S4によ
つて加算する。この加算器S4の出力を、前述したモデル
におけるばね・質量系のねじれ角度θ３の推定値 とすることによつて、本実施例における推定動作を、実
現することができる。Next, the output 1 of the rotation speed detector 4 is multiplied by the coefficient unit J31 and the output z1 of the integrator I1 is added by the adder S3. The output of this adder S3 is the estimated value of the derivative of the torsion angle θ3 of the spring-mass system in the model described above. And In addition, the output 1 of the rotation speed detector 4 has a coefficient unit J41
The calculation result multiplied by and the output z2 of the integrator I2 are added by the adder S4. The output of this adder S4 is the estimated value of the torsion angle θ3 of the spring-mass system in the model described above. By doing so, the estimation operation in the present embodiment can be realized.
第５図は本発明の一実施例の制御系を説明するブロツク
図である。本制御系においては、第１図示の回転角度検
出器３の出力θ１、回転速度検出器４の出力１および
前記状態推定器の出力である第２図示のモデルにおける
ばね・質量系のねじれ角度θ３およびその微分値３の
計４つの値を用いて制御動作が行なわれる。FIG. 5 is a block diagram for explaining the control system of the embodiment of the present invention. In this control system, the output θ1 of the rotation angle detector 3 shown in the first illustration, the output 1 of the rotation speed detector 4 and the output of the state estimator, ie, the torsion angle θ3 of the spring / mass system in the model shown in the second illustration. The control operation is performed using a total of four values including the differential value 3 and the differential value 3.
本制御系においては、減算器S1によつて、電動機１の回
転角度の指令値θｒから電動機１の回転角度検出器３の
出力θ１を減算して、これらの角度の偏差を求める。減
算器S1の出力である前記角度偏差信号に、係数器Kpを乗
じた演算結果から、下記の各演算結果を減算器S2によつ
て減算する。演算結果とは、電動機１の回転速度検出器
４の出力１に係数器Kvを乗じた演算結果、ならびに前
記状態推定器の出力である第２図示のモデルにおけるば
ね・質量系のねじれ角度θ３の推定値 に係数器Kpsを乗した演算結果、および前記状態推定器1
1の出力である第２図示のモデルにおけるばね・質量系
のねじれ角度θ３の微分値の推定値 に係数器Kvsを乗じた演算結果を加算器S5によつて加算
した値である。このような減算動作による減算器S2の出
力を、電動機１の駆動回路２に対する電流の指令値u1と
する。したがつて指令値u1は、下記の第13式のように表
現できる。In the present control system, the subtracter S1 subtracts the output θ1 of the rotation angle detector 3 of the electric motor 1 from the command value θr of the rotation angle of the electric motor 1 to obtain the deviation of these angles. The angle deviation signal output from the subtracter S1 is multiplied by a coefficient unit Kp, and the following calculation results are subtracted by the subtractor S2. The calculation result is the calculation result obtained by multiplying the output 1 of the rotation speed detector 4 of the electric motor 1 by the coefficient unit Kv, and the output of the state estimator of the torsion angle θ3 of the spring / mass system in the model shown in FIG. Estimated value Multiply the coefficient unit Kps by the calculation result, and the state estimator 1
Estimated value of derivative of torsion angle θ3 of the spring-mass system in the model shown in the second diagram, which is the output of 1 Is a value obtained by adding the calculation result obtained by multiplying the coefficient multiplier Kvs by the adder S5. The output of the subtractor S2 by such a subtraction operation is set as the current command value u1 for the drive circuit 2 of the electric motor 1. Therefore, the command value u1 can be expressed as the following thirteenth expression.
以上のように本実施例において、第１図示の構成に関し
て、減速機５、トルク伝達機構６およびアーム７などか
らなるばね・質量系のねじれ（第２図示の機械的等価回
路におけるばね９の軸線まわりのねじれ）によつて発生
するアーム７先端の振動や、アーム７の先端の位置決め
誤差を消去するにあたり、第５図示のような制御系を構
成することにより、第１図示の構成にたとえばセンサな
どを追加することなく、たとえば計算機のプログラムま
たは電気回路などによって実現される演算処理過程によ
つて、この消去を行なうようにできる。また最適レギユ
レータ理論によつて、ロボツトの姿勢変化による諸特性
の変動に影響を受けない制御動作を得ることができる。 As described above, in the present embodiment, with respect to the configuration shown in the first drawing, the torsion of the spring-mass system including the speed reducer 5, the torque transmission mechanism 6, the arm 7, etc. (the axis line of the spring 9 in the mechanical equivalent circuit shown in the second drawing) In eliminating the vibration of the tip of the arm 7 caused by the twisting around) and the positioning error of the tip of the arm 7, a control system as shown in FIG. It is possible to perform this erasing by adding a computer program or an arithmetic processing process realized by an electric circuit without adding the above. In addition, the optimal reguulator theory makes it possible to obtain a control operation that is not affected by changes in various characteristics due to changes in the robot attitude.
回転角度検出器３が前述したばね要素の後段に設置され
ている場合でも、容易に状態推定器を設計して制御系を
構成することができ、アーム７の振動および位置決め誤
差を消去することができる。なお状態推定器とは、いわ
ゆる状態観測器と称されているものと同一構成である。Even when the rotation angle detector 3 is installed in the latter stage of the above-mentioned spring element, the state estimator can be easily designed to configure the control system, and the vibration and the positioning error of the arm 7 can be eliminated. it can. The state estimator has the same structure as what is called a state observer.
効 果 以上のように本発明に従えば、電動機軸の一端に固定さ
れた回転速度検出器および回転変位検出器によつて電動
機軸の回転速度および回転変位が検出される。また電動
機軸の他端には、減速用歯車が結合され、減速用歯車の
出力軸がトルク伝達構成を介してアームを駆動する。Effect As described above, according to the present invention, the rotational speed and the rotational displacement of the electric motor shaft are detected by the rotational speed detector and the rotational displacement detector fixed to one end of the electric motor shaft. A reduction gear is coupled to the other end of the electric motor shaft, and the output shaft of the reduction gear drives the arm via the torque transmission structure.
このような構成を有する産業用ロボツトにおいて、前記
電動機の回転子、減速歯車、トルク伝達機構およびアー
ムによつて構成される軸系をばね・質量からなる系と仮
定し電動機電流指令値u1と電動機回転変位θ１とその電
動機回転速度１とに基づいて、ばね・質量系のねじれ
角度θ３の推定値 とねじれ角度θ３の時間微分地３の推定値 とを状態推定器11で求めて加算器S5で加算し、これによ
つてアーム回転変位θ２およびアーム回転速度２に対
応する値を求め、こうして電動機回転変位θ１、電動機
回転速度１、アーム回転変位θ２およびアーム回転速
度２の４個の状態の全てフイードバツクすることがで
き、こうして優れた制御性能を実現することが可能にな
る。こうしてアーム回転変位およびアーム回転速度のよ
うにセンサで直接計測することができない状態量を、上
述のように状態推定器11で推定して求めるようにし、構
成を簡略化することもまた可能となる。In the industrial robot having such a configuration, it is assumed that the shaft system constituted by the rotor of the electric motor, the reduction gear, the torque transmission mechanism and the arm is a system including springs and masses, and the electric motor current command value u1 and the electric motor. Estimated value of the torsion angle θ3 of the spring / mass system based on the rotational displacement θ1 and its motor rotation speed 1. And the estimated value of the time derivative 3 of the twist angle θ3 Are calculated by the state estimator 11 and are added by the adder S5 to obtain the values corresponding to the arm rotational displacement θ2 and the arm rotational speed 2, and thus the electric motor rotational displacement θ1, the electric motor rotational speed 1, and the arm rotational displacement All four states of θ2 and arm rotation speed 2 can be fed back, and thus excellent control performance can be realized. In this way, the state quantity that cannot be directly measured by the sensor, such as the arm rotation displacement and the arm rotation speed, is estimated and obtained by the state estimator 11 as described above, and the configuration can be simplified. .
特に本発明では、上述のように電動機回転変位θ１、電
動機回転速度θ１、アーム回転変位θ２（ばね・質量系
のねじれ角度θ３の推定値 と等価）およびアーム回転速度２（ばね・質量系のね
じれ角度θ３の時間微分値３の推定値 と等価）の合計４個の状態の全てをフイードバツクする
ことによつて、初めて、減衰がよく、したがつて振動が
なく、しかも速度の速い応答を得ることができるように
なる。もしも仮に、電動機回転変位θ１と電動機回転速
度１の２個だけの状態のフイードバツクをする構成で
あれば、調整することができる範囲が非常に狭く、少し
ゲインを上げるだけで、すぐに応答の減衰が悪くなつて
しまうという問題がある。またばね・質量系のねじれ角
度θ３の推定値 だけのフイードバツクでは、減衰はよくなるけれども、
応答速度を向上することはできない。さらにねじれ角度
の時間微分値３の推定値 だけのフイードバツクでは、ゲインを上げていつても、
応答が遅くなるだけで、振動の減衰そのものをよくする
ことはできない。本発明では、上述のように合計４個の
状態の全てのフイードバツクすることによつて、初めて
減衰がよくかつ速度の速い応答を得ることができるよう
になるのである。Particularly, in the present invention, as described above, the electric motor rotational displacement θ1, the electric motor rotational speed θ1, the arm rotational displacement θ2 (the estimated value of the torsion angle θ3 of the spring / mass system). And arm rotation speed 2 (estimated value of time derivative 3 of torsion angle θ3 of spring-mass system) By feeding back all four states (equivalent to the above), it is possible to obtain a good damping, thus vibration-free and fast response for the first time. If the feedback back is set in the two states of the motor rotational displacement θ1 and the motor rotational speed 1, the adjustable range is very narrow and the response will be attenuated immediately by increasing the gain a little. There is a problem that it gets worse. Estimated value of torsion angle θ3 of spring / mass system With only feedback, the damping is better,
The response speed cannot be improved. Estimated value of time derivative 3 of twist angle With only feedback, even if you increase the gain,
It is not possible to improve the vibration damping itself, only the response becomes slow. In the present invention, it is possible to obtain a response with good damping and high speed only by performing all the feedback back of the four states as described above.
第１図は本発明の一実施例の構成を示す斜視図、第２図
は第１図示の構成の機械的等価回路図、第３図は本発明
の一実施例の構成を説明するブロツク図、第４図は状態
推定器を計算機のプログラムまたは電気回路で実現され
た構成のブロツク図、第５図は本発明の一実施例の制御
系の構成を説明するブロツク図である。 １……電動機、1a……出力軸、２……駆動回路、３……
回転角度検出器、４……回転速度検出器、５……減速
機、６……トルク伝達機構、７……アーム、θ1,１…
…電動機１の出力軸1aの回転角およびその微分値、θ3,
３……アーム７のねじれ量およびその微分値、u1……
電動機１の駆動回路２に対する電流の指令値、 S1〜S4……加算器、F11,F12,F21,F22;G11,G21;J31,J41;
W11,W12,W21,W22;Kps,Kvs……係数器FIG. 1 is a perspective view showing the structure of an embodiment of the present invention, FIG. 2 is a mechanical equivalent circuit diagram of the structure shown in FIG. 1, and FIG. 3 is a block diagram for explaining the structure of the embodiment of the present invention. FIG. 4 is a block diagram of a configuration in which the state estimator is realized by a program of a computer or an electric circuit, and FIG. 5 is a block diagram illustrating the configuration of a control system of an embodiment of the present invention. 1 ... motor, 1a ... output shaft, 2 ... driving circuit, 3 ...
Rotation angle detector, 4 ... Rotation speed detector, 5 ... Reducer, 6 ... Torque transmission mechanism, 7 ... Arm, θ1,1 ...
The rotation angle of the output shaft 1a of the electric motor 1 and its differential value, θ3,
3 ... twist of arm 7 and its differential value, u1 ...
A command value of current to the drive circuit 2 of the electric motor 1, S1-S4 ... Adder, F11, F12, F21, F22; G11, G21; J31, J41;
W11, W12, W21, W22; Kps, Kvs ... coefficient unit
───────────────────────────────────────────────────── フロントページの続き (72)発明者 宇野 知之 兵庫県明石市川崎町１番１号 川崎重工業 株式会社明石工場内 (72)発明者 久保 貞夫 兵庫県明石市川崎町１番１号 川崎重工業 株式会社明石工場内 (56)参考文献 特開 昭54−109118（ＪＰ，Ａ) 特開 昭60−69712（ＪＰ，Ａ) 特開 昭56−153406（ＪＰ，Ａ) 特開 昭59−136815（ＪＰ，Ａ) ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Tomoyuki Uno 1-1 Kawasaki-cho, Akashi-shi, Hyogo Kawasaki Heavy Industries Ltd. Akashi factory (72) Sadao Kubo 1-1-1 Kawasaki-cho, Akashi-shi, Hyogo Kawasaki Heavy Industries Akashi Factory Co., Ltd. (56) Reference JP 54-109118 (JP, A) JP 60-69712 (JP, A) JP 56-153406 (JP, A) JP 59-136815 ( JP, A)
Claims (1)
回転角度検出器３が結合されるとともに減速歯車５が結
合され、該減速歯車５の出力軸がトルク伝達機構６を介
してアーム７を駆動するような構造を有する産業用ロボ
ツトと、 電動機１の前記軸の回転角度の指令値θｒを発生する手
段と、 前記回転角度の指令値θｒから回転角度検出器３によつ
て検出される電動機回転変位θ１の出力を減算する第１
減算器S1と、 電動機電流指令値u1を導出する第２減算器S2と、 前記電動機１の回転子、減速歯車５、トルク伝達機構６
およびアーム７によつて構成される軸系をばね・質量か
らなる系と仮定し、電動機電流指令値u1、電動機回転変
位θ１、電動機回転速度１、および外部トルクu2から
前記ばね・質量系のねじれ角度θ３の推定値 と、そのねじれ角度θ３の時間微分値３の推定値 とを求める状態推定器11と、 前記ねじれ角θ３の推定値 と前記時間微分値３の推定値 とを加算する加算器S5とを含み、 第２減算器S2は、第１減算器S1の出力から、回転速度検
出器４の出力１と加算器S5の出力とを減算して前記電
動機電流指令値u1を導出して電動機１に与えることを特
徴とする産業用ロボツトの制御装置。1. A rotation speed detector 4 and a rotation angle detector 3 are connected to a shaft of an electric motor 1 and a reduction gear 5 is connected thereto, and an output shaft of the reduction gear 5 is armed via a torque transmission mechanism 6. An industrial robot having a structure for driving 7, a means for generating a command value θr of the rotation angle of the shaft of the electric motor 1, and a rotation angle detector 3 detecting the command value θr of the rotation angle. First subtracting the output of the motor rotational displacement θ1
Subtractor S1, second subtractor S2 for deriving electric motor current command value u1, rotor of electric motor 1, reduction gear 5, torque transmission mechanism 6
Assuming that the shaft system configured by the arm 7 and the arm 7 is a system including a spring and a mass, the motor current command value u1, the motor rotational displacement θ1, the motor rotation speed 1, and the external torque u2 are used to twist the spring and mass system. Estimated value of angle θ3 And the estimated value of the time derivative 3 of the twist angle θ3 And a state estimator 11 for obtaining and an estimated value of the twist angle θ3. And the estimated value of the time differential value 3 The second subtractor S2 subtracts the output 1 of the rotation speed detector 4 and the output of the adder S5 from the output of the first subtractor S1 to add the A control device for an industrial robot characterized in that a value u1 is derived and given to the electric motor 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP60097525A JPH07120212B2 (en) | 1985-05-07 | 1985-05-07 | Industrial robot controller |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP60097525A JPH07120212B2 (en) | 1985-05-07 | 1985-05-07 | Industrial robot controller |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS61255415A JPS61255415A (en) | 1986-11-13 |
| JPH07120212B2 true JPH07120212B2 (en) | 1995-12-20 |
Family
ID=14194668
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP60097525A Expired - Fee Related JPH07120212B2 (en) | 1985-05-07 | 1985-05-07 | Industrial robot controller |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH07120212B2 (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014034101A (en) * | 2012-08-10 | 2014-02-24 | Toshiba Corp | Robot control device |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0638212B2 (en) * | 1986-08-14 | 1994-05-18 | 川崎重工業株式会社 | Object position control device |
| JPH0392911A (en) * | 1989-09-06 | 1991-04-18 | Fanuc Ltd | Robot control method for sliding mode control |
| JP2016078193A (en) * | 2014-10-20 | 2016-05-16 | 株式会社小松製作所 | Machine tool, laser beam machine, and method for control of machine tool |
| CN107107336B (en) * | 2014-11-18 | 2021-04-02 | 柿子技术公司 | Robot adaptive placement system with end effector position estimation |
| JP6986373B2 (en) * | 2017-06-21 | 2021-12-22 | 川崎重工業株式会社 | Robot system and control method of robot system |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE2802224C2 (en) * | 1978-01-19 | 1982-06-09 | Siemens AG, 1000 Berlin und 8000 München | Circuit arrangement for proportional-integral speed control of a work machine driven by an electric motor with variable load |
| JPS56153406A (en) * | 1980-04-30 | 1981-11-27 | Mitsubishi Heavy Ind Ltd | State estimating system for plant |
| JPS6069712A (en) * | 1983-09-27 | 1985-04-20 | Matsushita Electric Ind Co Ltd | Oscillation controller |
-
1985
- 1985-05-07 JP JP60097525A patent/JPH07120212B2/en not_active Expired - Fee Related
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014034101A (en) * | 2012-08-10 | 2014-02-24 | Toshiba Corp | Robot control device |
Also Published As
| Publication number | Publication date |
|---|---|
| JPS61255415A (en) | 1986-11-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US4989161A (en) | Control unit for a multi-degree-of freedom manipulator | |
| KR100425819B1 (en) | Mechanical vibration detection device and vibration suppression control device | |
| WO1991005296A1 (en) | Sliding mode control method | |
| JP4685071B2 (en) | Motor control device and motor control method | |
| JP3506157B2 (en) | Motor position control device | |
| US9796087B2 (en) | Control system for power unit | |
| KR20100023846A (en) | Movement controller for elastic robot structures | |
| US20160016310A1 (en) | Notch filter, external force estimator, motor control apparatus, and robotic system | |
| JP6604157B2 (en) | Resonance suppression controller in multi-inertia resonance system | |
| JP4860277B2 (en) | Vibration suppression control method and apparatus for multi-inertia resonance system | |
| JP3239789B2 (en) | Control device and control method | |
| JPH07120212B2 (en) | Industrial robot controller | |
| JP5208610B2 (en) | Characteristic identification method and motor control device for controlling inertial resonance system | |
| JPH10217173A (en) | Non-interferential control device of robot | |
| JP6496167B2 (en) | Tandem position control device | |
| Taira et al. | Motion and force control with a nonlinear force error filter for underwater vehicle-manipulator systems | |
| JP6845103B2 (en) | Control systems, control methods, and control programs | |
| Nagatsu et al. | High-order disturbance estimation using Kalman filter for precise reaction-torque control | |
| WO2018073873A1 (en) | Servo control device | |
| JP3856215B2 (en) | Speed control device | |
| JP2906255B2 (en) | Servo control device | |
| JP2838578B2 (en) | Motor control device, disturbance load torque estimation device | |
| JP5115783B2 (en) | stage | |
| JP4078396B2 (en) | Positioning control device | |
| Berninger et al. | The influence of structural dynamics on cascaded joint position control of a flexible beam with a compliant gear |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |