• 专利附录
  • 专利说明
  • 权利要求
  • 类似技术
  • 同族专利
  • 引用文献
  • 法律状态
  • 优先权
    • 专利摘要:

    公开号:WO1992014195A1
    申请号:PCT/JP1992/000120
    申请日:1992-02-06
    公开日:1992-08-20
    发明作者:Nobutoshi Torii；Ryo Nihei；Tetsuaki Kato
    申请人:Fanuc Ltd；
    IPC主号:G05B5-00
    专利说明:
    [0001] 明 細 書 制振制御装置 技術分野
    [0002] 本発明はロボッ ト等の低周波の振動系を有する制御対象に 対する制振制御装置に関し、 特にオブザーバを使用 して振動 を低減した制振制御装置に関する。 背景技術
    [0003] ロボッ ト等の低周波の振動系を有する制御対象では、 ァー ムの先端での振動が作業上非常に問題となる。 例えば、 位置 決め時においては、 先端の振動がおさまるまでは次の作業を 行う こ とはできず、 サイ クルタイ ムが悪化する。 通常これら の振動を防止するには、 系のサーボゲイ ンを下げて、 ロボ ッ トをゆつ く り停止させる こ とにより対処してきた。
    [0004] しかし、 サ一ボゲイ ンを小さ く する こ とにより、 位置決め 時間がかかり、 サイ クルタ イ ムが悪く なる。 また、 サーボゲ イ ンを小さ く する こ とにより、 サーボ剛性が低く なり、 ァー ム先端の軌跡精度が低下し、 アーク溶接、 レーザ切断等に使 用する ロボ ッ トでは加工精度が低下する結果となる。 発明の開示
    [0005] 本発明はこのような点に鑑みてなされたものであり、 ねじ れ量及びねじれ速度に特殊なフ ィ ルタをかけた状態フ ィ一 ド バックによって振動を低減する制振制御方式を提供すること を目的とする。
    [0006] 本発明によれば、 制御対象に対し目標動作に応じた制御指 令を与え、 該制御指令に応答する制御対象の観測値を制御指 令にフィードバックさせて制御対象の観測値が該目標動作に 一致しかつ制御対象の振動を抑制するように制御する制振制 御装置であって、 該制御対象内のねじれバラメータを決定す るねじれパラメータ決定手段と、 該制御対象の振動を抑制す るように決定された伝達関数を該ねじれパラメータに乗じて 該制御指令にフィー ドバックするねじれフィー ドバック手段 とを具備する制振制御装置が提供される。 図面の簡単な説明
    [0007] 図 1 は、 本発明を実施するためのロボッ ト システムのハー ドゥェァの構成図 ；
    [0008] 図 2 は、 本発明のサーボモータの制御のプロ ック図である。 発明を実施するための最良の形態
    [0009] 以下、 本発明の一実施例を図面に基づいて説明する。
    [0010] 第 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 (パネ定数 Kc ) 、 ダンピング成分 2 5 (ダ ンビング定数 B k ) を模式的に表している。 また、 D S P 1 1 は後述の トルク指令 T及びサーボモータ 2 2 の速度 (i) から、 ねじれ量 ε、 ねじれ速度 ε ( を推定するオブザーバ 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 ではこの速度指令 Vd から後述するフィー ドバック量 Fが差し引かれる。 フ ィ 一ドバック量 Fについては後で詳述する。
    [0011] 加箕器 3 2 a の出力は加算器 3 3に送られる。 加算器 3 3 ではサーボモータ 2 2の速度指令 V d とサーボモータ 2 2か らのフィー ドバック速度 CI) との差分をとり、 その差分は 要素 3 4に送られる。 また、 加算器 3 3では、 位置指令 d を微分要素 3 0で微分した出力 6 d 1) が加算される。 この 微分要素 3 0 はフィー ドフォワー ド · ループを構成している。 要素 3 4では、 速度制御ループゲイ ン Kv をかけ、 トルク指 令 Τとし、 トルク指令 Τは要素 3 5に送られる。 速度制御ル 一プの带域は位置ループの帯域にく らべ 1桁以上大きいもの とする。 要素 3 5 はサーボモータ 2 2に対応する要素であり、 その岀カはサーボモータ 2 2 の速度 Θ い 、 であり、 その速度 を積分要素 3 7で積分 （ 1 / s ) した出力はサーボモータ 2 2の位置 0である。 ここで、 s はラプラス変数である。
    [0012] オブザーバ 3 6 は同一次元オブザーバであり、 トルク指令 Τとサーボモータ 3 5 0速度 0 (,) からサーボモータ 3 5 の 位置 とアーム 2 6の位置 6 t との位置偏差であるねじれ量 s、 サーボモータ 3 5 の速度 6 とアームの速度 0
    [0013] との速度偏差であるねじれ速度 ε (1) を推定する。
    [0014] このねじれ量 ε は要素 3 8に送られ、 係数 （ K^ s 十 C ) / ( s ナ A ) をかけられる。 また、 ねじれ速度 ε (1> は要 素 3 9で、 係数 〔 K2 ( s 十 C ) ノ （ s + A) 〕 をかけられ る。 要素 3 8 と要素 3 9の出力は加算器 4 0で加算され、 フ イ ー ドバッ ク量 Fとして、 速度指令 Vd からさし引かれる。 次に図 2での伝達関数を考えると、 （ 1 ) 式が得られる。 (ただし、 速度制御ループの応答性は位置ループに比較して 非常に速いため伝達特性を 1 と仮定する。 ）
    [0015] { θ A - Θ ) * C + ^ <, (1) - K , * Q * ε
    [0016] 一 Kz * Q * ε ( = θ (1 ( 1 ) ただし、 Q = ( s + A ) / ( s + C )
    [0017] e = θ - Θ t
    [0018] ； 1 ) = Θ < 1 ) 一 Θ ( 1 ) である。
    [0019] 次に図 1 に示したロボッ トの動作をバネ、 ダンバの振動モ デルについて考察する こ とにより記述する。 ロボッ 卜の減速 器はほぼこのモデルで表示できる。 こ こで、 各係数を以下の 通り とする。
    [0020] J t ： アームのイ ナーシャ
    [0021] Θ t ： アームの先端位置
    [0022] B k ： 減速器系のダンビング定数
    [0023] Kc ： 滅速器系のパネ定数
    [0024] J ： 減速器から下のイ ナーシャ
    [0025] Θ ： サーボモータの位置
    [0026] サ一ボモータ 2 2の位置 からアーム 2 6 の先端の位置 t までの伝達関数を考える と、 （ 2 ) 式が得られる。
    [0027] ( Θ t / 6 ) = ( B k * s + K c ) / ( J t * s 2
    [0028] + B k * s + K c ) … （ 2 ) こ こで、 （ 2 ) 式を について解き （ 1 ) 式に代入する と、 ( 3 ) 式が得られる。
    [0029] ( ^ t / ^d ) = ( Bk * s + Kc ) /R i
    [0030]
    [0031] + 〔 ( J t * K,)/ ( s + A) 〕 * s 2
    [0032] -r- J t * s z + Bk * s + KC - ( 3 )
    [0033] ( 3 ) 式で、 （ s 十 A) が振動周波数付近で Sとみなせる 程度に Aを小さ く とる、 すなわちアームの固有振動数より も 小さい値にとると、 （ 3 ) 式は （ 4 ) 式となる。
    [0034]
    [0035] R z = C J t + J t * Kz 〕 * s 2
    [0036]
    [0037] となる。
    [0038] ( 4 ) 式から明らかなように、 K, を変えることにより、 ダンピング項を単独で変化させることができ、 振動を抑制す ることができる。 すなわち、 K, を大き くすることにより、 ダンピング項を大き くでき、 振動を抑制することができる。 また、 Kz を変えることにより、 慣性モーメ ン トを変える 二 とができ、 振動周波数を変えることができる。 すなわち、 Κ:_- を負の値にすることにより、 振動周波数を高めて、 位置決め を速くすることができる。
    [0039] ここで、 （ s 十 Α) を最初から s としないのは、 状態変数 の積分がでてく るため、 もし、 オブザーバで推定する場合、 パラメータが制御対象と一致していなくて状態変数にオフセ ッ トが乗っていた場合に、 それが蓄積されるのを防止するた めである。 このように、 図 2 の要素 3 8 , 3 9 に示すフ ィ ー ドバッ ク を行う こ とによ り、 振動を抑制し、 位置決め時間を短縮する こ とができる。 また、 サーボループゲイ ンを小さ く する こ と な く、 軌跡精度も向上する。
    [0040] 最後にサ一ボモータ 3 5 の トルク指令 Tとサーボモータ 3 5 の速度 からねじれ量 ε および ε を推定するォブ ザ一バ 3 6 を実現するための演算について説明する。
    [0041] 図 1 に示したように、 ロボッ トをバネ定数 K c のパネ 2 4 とダンピング定数 B k のダンバ 2 5 とによるモデルで表わす, モータ側の定数を以下の様に表わし、
    [0042] J ： ロ一タイ ナーシャ
    [0043] A n ： 粘性係数
    [0044] モータ側に着目 して運動方程式を立てる と、
    [0045] T = J m * θ (2 ) + Β κ * ( θ ( 1 ) 一 θ η )
    [0046] 十 K c * ( e — ） + Am * e (1) - ( 5 ) が得られる。 また、 負荷側に着目 して運動方程式を立てる と、 0 = J t * Θ t l z ) + B κ * ( Θ t li 一 θ い )
    [0047] + c * ( Θ t - θ ) … （ 6 ) が得られる。
    [0048] ε = θ - Θ t - ( 7 ) と置き換え、 （ 5 ) ( 6 ) 式の両辺について （ 5 ) „ 一
    [0049] ( 6 ) / J t を計算する と、 T B B K K c ( ( I )
    [0050] ·= ε (2) + ) ε + (
    [0051] J m J J t
    [0052] A m
    [0053] :1 >
    [0054] J„
    [0055] B B K c K
    [0056] (2) = =一 ( ) e 1 ) ― ) ε
    [0057] J J J
    [0058] T
    [0059] Θ :1 )
    [0060] 十 ( 8 )
    [0061] J m J m
    [0062] が得られる。 また、 （ 5 ) 式を e (2> について解いて （ 7 ) 式を代入すると、
    [0063] B κ A
    [0064] Θ <2) = ： I ) Θ ( I )
    [0065] J J J
    [0066] T
    [0067] ( 9 )
    [0068] J が得られる。
    [0069] 犾態変数 X ί , 2 . X 3 を
    [0070] X I = ε (1
    [0071] X 2 = ε
    [0072] X 3 = Θ (1>
    [0073] にとると、 （ 8)(9 ) 式より、 状態方程式
    [0074] と表わされる。 また、 観測可能な状態変数は x 3 であるから、 観測方程式は
    [0075] Υ = ( 0 0 1 ) [ X 1 X 2 X 3 ]
    [0076] と表わされる。 こ こでこれらの式を Ζ変換して
    [0077] X ( η + 1 ) = A ζ * X ( n ) - B z * T
    [0078] Y = C * X ( n )
    [0079] と表わす。 ただし、
    [0080]
    [0081] である。 こ こで X ( n ) の推定値を X ( n ) と して同一次元 オブザーバを組 と、
    [0082] X ( n + l ) = ( A z 一 K . C ) * X ( n ) 十 B z * T
    [0083] - K · Y の計算により、 推定値 X ( n ) が算出される。 ただし Kは A z — K · Cが安定になるように選択する。
    [0084] 上記の説明では、 制御対象をロボッ トとしたが、 ロボッ ト 以外の低周波の振動系を有する制御対象にも同様に適用する ことができる。
    [0085] また、 ねじれ量及びねじれ速度を求めるのにオブザーバを 使用したが、 これらの値を直接機構部の先端に位置及び速度 の検出器を設けて、 パルスコーダの出力と比較することによ り求めることもできる。 その場合は検出器を必要とする力 ;、 これらのねじれ量等は正確な値が得られる。
    [0086] 以上説明したように本発明では、 機構部を舍めた制御系 O ダンビング項とィナーシャ項が独立に制御できるような状態 フィードバックを設けたので、 サーボ系のゲイ ンを小さ く す ることな く、 振動を抑制することができる。 このために、 位 置決め時間が短く なり、 軌跡精度が向上する。
    权利要求:
    Claims請 求 の 範 囲
    1. 制御対象に対し目標動作に応じた制御指令を与え、 該 制御指令に応答する制御対象の観測値を制御指令にフ ィ一ド バックさせて制御対象の観測値が該目標動作に一致しかつ制 御対象の振動を抑制するように制御する制振制御装置であつ て、
    該制御対象内のねじれバラメータを決定するねじれパラメ ータ決定手段と、
    該制御対象の振動を抑制するように決定された伝達関数を 該ねじれパラメータに乗じて該制御指令にフィー ドバックす るねじれフィー ドバック手段とを具備する制振制御装置。
    2. 前記目標動作は制御対象の目標位置と目標速度とを舍 み、
    前記制御対象の観測値は制御対象の観測位置と観測速度と を舍み、
    該制御対象の観測位置と目標位置との偏差に所定の位置ル ープゲイ ンを乗じて前記制御指令にフィ一ドバックする位置 フ ィー ドバッ ク手段と、
    該制御対象の観測速度と目標速度との偏差に所定の速度ル ープゲイ ンを乗じて前記制御指令にフィー ドバックする速度 フィー ドバッグ手段とをさ らに具備する請求の範囲第 1 項に 記載の制振制御装置。
    3. 前記ねじれパラメータはねじれ量 e とその一次時間微 分であるねじれ速度 ε ( 1 を舍み、 前記ねじれフ ィー ドハツ ク手段は式
    - C K I * { ( s + C ) / s + A ] ε
    + Κ2 * { ( s + C ) / s + A } e (I ) 〕
    で算岀される量を制御指令にフイードバックするものであり ただし、 Cは前記位置フィ一ドバック手段の位置ループゲイ ン、 s はラプラス変数、 A, Κ , , K2 は制御対象の振動を 抑制するように決定された定数である請求の範囲第 2項に記 載の'制振制御装置。
    4. 前記ねじれパラメータ決定手段は、 制御対象への制御 指令と制御対象の観測値とから前記ねじれ量 sおよびねじれ 速度 s (" を推定するオブザーバである請求の範囲第 3項に 記載の制振制御装置。
    5. 前記ねじれパラメータ浃定手段は、 制御対象における ねじれ量 sおよびねじれ速度 ε (1) を検出するねじれ検出手 段を有する請求の範囲第 3項に記載の制振制御装置。
    6. 前記定数 Αは前記制御対象の固有振動数より も小さい 値である請求の範囲第 3項に記載の制振制御装置。
    7. 前記制御対象はサーボモータによつて駆動されるァー ムを有し、 前記ねじれ量 εおよびねじれ速度 ε (1) はそれぞ れ該サーボモータの位置と該アームの先端位置との偏差およ びその一次時間微分である請求の範囲第 3項に記載の制振制
    类似技术:
    公开号 | 公开日 | 专利标题
    Walrath1984|Adaptive bearing friction compensation based on recent knowledge of dynamic friction
    Vos et al.1990|Dynamics and nonlinear adaptive control of an autonomous unicycle: Theory and experiment
    Luh1983|Conventional controller design for industrial robots—a tutorial
    Slotine et al.1988|Adaptive manipulator control: A case study
    EP0417312B1|1996-01-10|Feedforward control unit for servomotor
    US5442270A|1995-08-15|Adaptive sliding mode control method for object of control including spring system
    JP3739749B2|2006-01-25|制御装置
    US4663703A|1987-05-05|Predictive model reference adaptive controller
    Kamalzadeh et al.2007|Compensation of axial vibrations in ball screw drives
    DE4318923C2|2002-10-31|Positionsregeleinrichtung zum Regeln eines elektrischen Motors
    EP1505463B1|2011-08-24|Position control device and position control method for machine tools
    JP3899526B2|2007-03-28|位置制御装置
    JP4879173B2|2012-02-22|電動機制御装置
    US5179514A|1993-01-12|Method and apparatus for trajectory control of robot manipulators or the like
    JP3297643B2|2002-07-02|送り駆動系のサーボ制御方法およびサーボ制御装置
    EP0280324B1|1993-06-23|Nonlinear control unit for a multi-degree-of freedom manipulator
    CN101640511B|2013-05-29|电动机控制装置
    US7019482B2|2006-03-28|Motion controller having sliding mode controller
    EP1291747B1|2010-03-24|Positioning servocontroller
    CN100578412C|2010-01-06|机械位置控制装置
    US4727303A|1988-02-23|Positional control method and system utilizing same
    US5056038A|1991-10-08|Apparatus for effecting coordinated position/force control for a manipulator
    EP0321579A1|1989-06-28|Articulated robot controller
    US5495158A|1996-02-27|Apparatus and method used with AC motors for controlling motor operation
    DE3708266B4|2004-12-09|Servosystem mit Nachführung
    同族专利:
    公开号 | 公开日
    EP0523252A4|1993-06-09|
    EP0523252A1|1993-01-20|
    JPH04255007A|1992-09-10|
    引用文献:
    公开号 | 申请日 | 公开日 | 申请人 | 专利标题
    JPS6337329B2|1980-02-21|1988-07-25|Tokyo Shibaura Electric Co||
    JPS5966705A|1982-09-14|1984-04-16|Vickers Inc|Power servo system|
    JPS6368903A|1986-09-10|1988-03-28|Fujitsu Ltd|Digital servo controller|
    JPH01157283A|1987-10-26|1989-06-20|Siemens Ag|Method of detecting and adjusting spring moment and differential revolution number|
    JPH01296301A|1988-05-25|1989-11-29|Fanuc Ltd|Method for controlling servo loop of industrial robot|CN101133228B|2005-03-11|2011-06-15|阿特拉斯科普科凿岩机股份公司|用于齿轮箱中输出轴的减震装置|JPS63314606A|1987-06-18|1988-12-22|Fanuc Ltd|Controller for articulated robot|DE3738968C1|1987-11-17|1988-09-01|Hans-Peter Schuermann|Hairdressing implement having a sound-damped central blower|
    EP0695606A1|1994-07-25|1996-02-07|Consorzio Per La Ricerca Sulla Microelettronica Nel Mezzogiorno|Fuzzy control process and device for positioning and quickly damping mechanical oscillations|
    DE19734208A1|1997-08-07|1999-02-11|Heidenhain Gmbh Dr Johannes|Verfahren und Schaltungsanordnung zur Ermittlung optimaler Reglerparamter für eine Drehzahlregelung|
    DE19846637A1|1998-10-09|2000-04-13|Heidenhain Gmbh Dr Johannes|Verfahren und Schaltungsanordnung zur automatischen Parametrierung eines schnellen digitalen Drehzahlregelkreises|
    JP4578732B2|2001-08-13|2010-11-10|インテリジェントマニュファクチャリングシステムズインターナショナル|工作機械送り系の制御装置|
    JP4272565B2|2003-07-18|2009-06-03|株式会社リコー|ベルト駆動制御装置及び画像形成装置|
    JP2005301508A|2004-04-08|2005-10-27|Fanuc Ltd|制御装置|
    JP5821210B2|2011-02-22|2015-11-24|セイコーエプソン株式会社|水平多関節ロボット及び水平多関節ロボットの制御方法|
    JP5613117B2|2011-07-20|2014-10-22|本田技研工業株式会社|弾性部材の変形速度演算装置および変形速度演算方法ならびに駆動装置|
    JP6097174B2|2013-08-05|2017-03-15|株式会社東芝|ロボット制御装置|
    JP2016078193A|2014-10-20|2016-05-16|株式会社小松製作所|工作機械、レーザ加工機、及び工作機械の制御方法|
    法律状态:
    1992-08-20| AK| Designated states|Kind code of ref document: A1 Designated state(s): KR US |
    1992-08-20| AL| Designated countries for regional patents|Kind code of ref document: A1 Designated state(s): DE IT SE |
    1992-09-30| WWE| Wipo information: entry into national phase|Ref document number: 1992904399 Country of ref document: EP |
    1993-01-20| WWP| Wipo information: published in national office|Ref document number: 1992904399 Country of ref document: EP |
    1994-03-02| WWW| Wipo information: withdrawn in national office|Ref document number: 1992904399 Country of ref document: EP |
    优先权:
    申请号 | 申请日 | 专利标题
    [返回顶部]