WO1992014195A1

WO1992014195A1 - Oscillation damper

Info

Publication number: WO1992014195A1
Application number: PCT/JP1992/000120
Authority: WO
Inventors: Nobutoshi Torii; Ryo Nihei; Tetsuaki Kato
Original assignee: Fanuc Ltd
Priority date: 1991-02-06
Filing date: 1992-02-06
Publication date: 1992-08-20
Also published as: EP0523252A4; EP0523252A1; JPH04255007A

Description

明細書制振制御装置技術分野

本発明はロボット等の低周波の振動系を有する制御対象に対する制振制御装置に関し、特にオブザーバを使用して振動を低減した制振制御装置に関する。背景技術

ロボット等の低周波の振動系を有する制御対象では、ァームの先端での振動が作業上非常に問題となる。例えば、位置決め時においては、先端の振動がおさまるまでは次の作業を行うことはできず、サイクルタイムが悪化する。通常これらの振動を防止するには、系のサーボゲインを下げて、ロボットをゆつくり停止させることにより対処してきた。

しかし、サ一ボゲインを小さくすることにより、位置決め時間がかかり、サイクルタイムが悪くなる。また、サーボゲインを小さくすることにより、サーボ剛性が低くなり、ァーム先端の軌跡精度が低下し、アーク溶接、レーザ切断等に使用するロボットでは加工精度が低下する結果となる。発明の開示

本発明はこのような点に鑑みてなされたものであり、ねじれ量及びねじれ速度に特殊なフィルタをかけた状態フィ一ドバックによって振動を低減する制振制御方式を提供することを目的とする。

本発明によれば、制御対象に対し目標動作に応じた制御指令を与え、該制御指令に応答する制御対象の観測値を制御指令にフィードバックさせて制御対象の観測値が該目標動作に一致しかつ制御対象の振動を抑制するように制御する制振制御装置であって、該制御対象内のねじれバラメータを決定するねじれパラメータ決定手段と、該制御対象の振動を抑制するように決定された伝達関数を該ねじれパラメータに乗じて該制御指令にフィードバックするねじれフィードバック手段とを具備する制振制御装置が提供される。図面の簡単な説明

図 1 は、本発明を実施するためのロボットシステムのハードゥェァの構成図；

図 2 は、本発明のサーボモータの制御のプロック図である。発明を実施するための最良の形態

以下、本発明の一実施例を図面に基づいて説明する。

第 1図は本発明を実施するためのロボットシステムのハードウエアの構成図である。ホストプロセッサ 1 はロボット全体を制御するプロセッサである。ホストプ πセッサ 1 からは口ボットの位置指令 Θ _d が共有 R A M 2に書き込まれる。なお、ホストプロセッサ 1 に結合される R O M , R A M等は省略してある。ロボットに内蔵されたサーボモータ 2 2を制御する D S P (ディジタル . シグナル ' プロセッサ） 1 1 は R 0 M 1 2のシステムプログラムに従って、サーボモータ 2 2を制御する。このため、 D S P 1 1 は共有 R AM 2の位置指令 6 _d を一定時間ごとに読み取る。 D S P 1 1 はこの位置指令 e _d と、サ —ボモータ 2 2に内蔵されたパルスコーダ 2 3からの位置フィ一ドバックとの差分によるエラ一量から速度指令を計算する。さらに、位置フィ一ドバックを微分して、速度フィ一ドバックを計算する。この速度フィ一ドバックと速度指令との差分からトルク指令を計算する。この速度指令とトルク指令はディジタルサ一ボ L S I ( D S L ) 1 4経由でサーボアンブ 2 1 に与えられ、サーボアンブ 2 1 はトルク指令を受けて、サーボモータ 2 2を駆動する。サーボモータ 2 2 は減速機を介して、アーム 2 6を駆動する。図では、減速機等の機械系のパネ成分 2 4 (パネ定数 K_c ) 、ダンピング成分 2 5 (ダンビング定数 B _k ) を模式的に表している。また、 D S P 1 1 は後述のトルク指令 T及びサーボモータ 2 2 の速度 ⁽ⁱ⁾ から、ねじれ量 ε、ねじれ速度 ε ⁽ を推定するオブザーバ 3 6の機能等を果たす。なお、上記および以下の記述において、 X ^(η) はバラメータ Xの η次の時間微分を表わしている。図 2 は本発明の制振制御方式でのサーボモータの制御のブロック図である。共有 R AM 2から読み込まれた位置指令 e _d は加算器 3 1 に送られ、パルスコーダ 2 3からの位置フィードバックとの差分を取り、その差分は要素 3 2 に送られる。要素 3 2では入力に位置ループゲイン Cをかけ、速度指令 V _d として加算器 3 2 a に送る。加算器 3 2 a ではこの速度指令 V_d から後述するフィードバック量 Fが差し引かれる。フィ一ドバック量 Fについては後で詳述する。

加箕器 3 2 a の出力は加算器 3 3に送られる。加算器 3 3 ではサーボモータ 2 2の速度指令 V _d とサーボモータ 2 2からのフィードバック速度 ^CI) との差分をとり、その差分は要素 3 4に送られる。また、加算器 3 3では、位置指令 _d を微分要素 3 0で微分した出力 6 _d ¹⁾ が加算される。この微分要素 3 0 はフィードフォワード · ループを構成している。要素 3 4では、速度制御ループゲイン K_v をかけ、トルク指令 Τとし、トルク指令 Τは要素 3 5に送られる。速度制御ル一プの带域は位置ループの帯域にくらべ 1桁以上大きいものとする。要素 3 5 はサーボモータ 2 2に対応する要素であり、その岀カはサーボモータ 2 2 の速度 Θ い、であり、その速度を積分要素 3 7で積分（ 1 / s ) した出力はサーボモータ 2 2の位置 0である。ここで、 s はラプラス変数である。

オブザーバ 3 6 は同一次元オブザーバであり、トルク指令 Τとサーボモータ 3 5 0速度 0 ^(,) からサーボモータ 3 5 の位置とアーム 2 6の位置 6 _t との位置偏差であるねじれ量 s、サーボモータ 3 5 の速度 6 とアームの速度 0

との速度偏差であるねじれ速度 ε ⁽¹⁾ を推定する。

このねじれ量 ε は要素 3 8に送られ、係数（ K^ s 十 C ) / ( s ナ A ) をかけられる。また、ねじれ速度 ε ^(1> は要素 3 9で、係数〔 K₂ ( s 十 C ) ノ（ s + A) 〕をかけられる。要素 3 8 と要素 3 9の出力は加算器 4 0で加算され、フイードバック量 Fとして、速度指令 V_d からさし引かれる。次に図 2での伝達関数を考えると、（ 1 ) 式が得られる。 (ただし、速度制御ループの応答性は位置ループに比較して非常に速いため伝達特性を 1 と仮定する。）

{ θ _A - Θ ) * C + ^ <, ⁽¹⁾ - K , * Q * ε

一 K_z * Q * ε ⁽ = θ ⁽¹ ( 1 ) ただし、 Q = ( s + A ) / ( s + C )

e = θ - Θ _t

； 1 ) = Θ < 1 ) 一 Θ ( 1 ) である。

次に図 1 に示したロボットの動作をバネ、ダンバの振動モデルについて考察することにより記述する。ロボッ卜の減速器はほぼこのモデルで表示できる。ここで、各係数を以下の通りとする。

J t ：アームのイナーシャ

Θ t ：アームの先端位置

B _k ：減速器系のダンビング定数

K_c ：滅速器系のパネ定数

J ：減速器から下のイナーシャ

Θ ：サーボモータの位置

サ一ボモータ 2 2の位置からアーム 2 6 の先端の位置 _t までの伝達関数を考えると、（ 2 ) 式が得られる。

( Θ _t / 6 ) = ( B _k * s + K c ) / ( J t * s ²

+ B _k * s + K c ) … （ 2 ) ここで、（ 2 ) 式をについて解き（ 1 ) 式に代入すると、 ( 3 ) 式が得られる。

( ^ t / ^d ) = ( B_k * s + K_c ) /R i

+ 〔 ( J t * K,)/ ( s + A) 〕 * s ²

-r- J t * s ^z + B_k * s + K_C - ( 3 )

( 3 ) 式で、（ s 十 A) が振動周波数付近で Sとみなせる程度に Aを小さくとる、すなわちアームの固有振動数よりも小さい値にとると、（ 3 ) 式は（ 4 ) 式となる。

R z = C J t + J t * Kz 〕 * s ²

となる。

( 4 ) 式から明らかなように、 K, を変えることにより、ダンピング項を単独で変化させることができ、振動を抑制することができる。すなわち、 K, を大きくすることにより、ダンピング項を大きくでき、振動を抑制することができる。また、 K_z を変えることにより、慣性モーメントを変える二とができ、振動周波数を変えることができる。すなわち、 Κ:_- を負の値にすることにより、振動周波数を高めて、位置決めを速くすることができる。

ここで、（ s 十 Α) を最初から s としないのは、状態変数の積分がでてくるため、もし、オブザーバで推定する場合、パラメータが制御対象と一致していなくて状態変数にオフセットが乗っていた場合に、それが蓄積されるのを防止するためである。このように、図 2 の要素 3 8 , 3 9 に示すフィードバックを行うことにより、振動を抑制し、位置決め時間を短縮することができる。また、サーボループゲインを小さくすることなく、軌跡精度も向上する。

最後にサ一ボモータ 3 5 のトルク指令 Tとサーボモータ 3 5 の速度からねじれ量 ε および ε を推定するォブザ一バ 3 6 を実現するための演算について説明する。

図 1 に示したように、ロボットをバネ定数 K c のパネ 2 4 とダンピング定数 B _k のダンバ 2 5 とによるモデルで表わす, モータ側の定数を以下の様に表わし、

J ：ロ一タイナーシャ

A n ：粘性係数

モータ側に着目して運動方程式を立てると、

T = J _m * θ ^{(2 )} + Β κ * ( θ ^{( 1 )} 一 θ ^η )

十 K _c * ( e — ） + A_m * e ⁽¹) - ( 5 ) が得られる。また、負荷側に着目して運動方程式を立てると、 0 = J t * Θ t ^{l z )} + B κ * ( Θ _t ^li 一 θ い )

+ _c * ( Θ _t - θ ) … （ 6 ) が得られる。

ε = θ - Θ _t - ( 7 ) と置き換え、（ 5 ) ( 6 ) 式の両辺について（ 5 ) „ 一

( 6 ) / J _t を計算すると、 T B B K K c ( ( I )

·= ε (2) + ) ε + (

J m J J t

A m

:1 >

J„

B B K c K

(2) = =一 ( ) e 1 ) ― ) ε

J J J

T

Θ :1 )

十 ( 8 )

J m J m

が得られる。また、（ 5 ) 式を e (^2> について解いて（ 7 ) 式を代入すると、

B κ A

Θ <2) = ： I ) Θ ( I )

J J J

T

( 9 )

J が得られる。

犾態変数 X ί , 2 . X 3 を

X I = ε ⁽¹

X 2 = ε

X 3 = Θ ^(1>

にとると、（ 8)(9 ) 式より、状態方程式

と表わされる。また、観測可能な状態変数は x ₃ であるから、観測方程式は

Υ = ( 0 0 1 ) [ X 1 X ₂ X 3 ]

と表わされる。ここでこれらの式を Ζ変換して

X ( η + 1 ) = A ζ * X ( n ) - B z * T

Y = C * X ( n )

と表わす。ただし、

である。ここで X ( n ) の推定値を X ( n ) として同一次元オブザーバを組と、

X ( n + l ) = ( A z 一 K . C ) * X ( n ) 十 B z * T

- K · Y の計算により、推定値 X ( n ) が算出される。ただし Kは A z — K · Cが安定になるように選択する。

上記の説明では、制御対象をロボットとしたが、ロボット以外の低周波の振動系を有する制御対象にも同様に適用することができる。

また、ねじれ量及びねじれ速度を求めるのにオブザーバを使用したが、これらの値を直接機構部の先端に位置及び速度の検出器を設けて、パルスコーダの出力と比較することにより求めることもできる。その場合は検出器を必要とする力 ^;、これらのねじれ量等は正確な値が得られる。

以上説明したように本発明では、機構部を舍めた制御系 O ダンビング項とィナーシャ項が独立に制御できるような状態フィードバックを設けたので、サーボ系のゲインを小さくすることなく、振動を抑制することができる。このために、位置決め時間が短くなり、軌跡精度が向上する。

Claims

請求の範囲

1. 制御対象に対し目標動作に応じた制御指令を与え、該制御指令に応答する制御対象の観測値を制御指令にフィ一ドバックさせて制御対象の観測値が該目標動作に一致しかつ制御対象の振動を抑制するように制御する制振制御装置であつて、

該制御対象内のねじれバラメータを決定するねじれパラメータ決定手段と、

該制御対象の振動を抑制するように決定された伝達関数を該ねじれパラメータに乗じて該制御指令にフィードバックするねじれフィードバック手段とを具備する制振制御装置。

2. 前記目標動作は制御対象の目標位置と目標速度とを舍み、

前記制御対象の観測値は制御対象の観測位置と観測速度とを舍み、

該制御対象の観測位置と目標位置との偏差に所定の位置ループゲインを乗じて前記制御指令にフィ一ドバックする位置フィードバック手段と、

該制御対象の観測速度と目標速度との偏差に所定の速度ループゲインを乗じて前記制御指令にフィードバックする速度フィードバッグ手段とをさらに具備する請求の範囲第 1 項に記載の制振制御装置。

3. 前記ねじれパラメータはねじれ量 e とその一次時間微分であるねじれ速度 ε ^{( 1} を舍み、前記ねじれフィードハツク手段は式

- C K I * { ( s + C ) / s + A ] ε

+ Κ₂ * { ( s + C ) / s + A } e ^{(I )} 〕

で算岀される量を制御指令にフイードバックするものでありただし、 Cは前記位置フィ一ドバック手段の位置ループゲイン、 s はラプラス変数、 A, Κ , , K₂ は制御対象の振動を抑制するように決定された定数である請求の範囲第 2項に記載の'制振制御装置。

4. 前記ねじれパラメータ決定手段は、制御対象への制御指令と制御対象の観測値とから前記ねじれ量 sおよびねじれ速度 s (" を推定するオブザーバである請求の範囲第 3項に記載の制振制御装置。

5. 前記ねじれパラメータ浃定手段は、制御対象におけるねじれ量 sおよびねじれ速度 ε ⁽¹⁾ を検出するねじれ検出手段を有する請求の範囲第 3項に記載の制振制御装置。

6. 前記定数 Αは前記制御対象の固有振動数よりも小さい値である請求の範囲第 3項に記載の制振制御装置。

7. 前記制御対象はサーボモータによつて駆動されるァームを有し、前記ねじれ量 εおよびねじれ速度 ε ⁽¹⁾ はそれぞれ該サーボモータの位置と該アームの先端位置との偏差およびその一次時間微分である請求の範囲第 3項に記載の制振制