WO1991019235A1

WO1991019235A1 - Systeme de regulation des caracteristiques de reponse d'une servocommande

Info

Publication number: WO1991019235A1
Application number: PCT/JP1991/000751
Authority: WO
Inventors: Nobutoshi Torii; Ryo Nihei; Tetsuaki Kato
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1990-06-07
Filing date: 1991-05-31
Publication date: 1991-12-12
Anticipated expiration: 1992-12-07
Also published as: EP0486694A1; EP0486694A4; JPH0442302A

Description

明細書コンプライアンス制御方式技術分野

本発明はサーボ系のコンプライアンス制御方式に関し、特にロボット等のサーボ系を制御するコンプライアンス制御方式に関 ^る。背景技術

位置制御あるいは軌道制御のみを対象としている溶接、塗装ロボット等は剛性が髙ぃほど、望ましい。一方、組立ロボット等では、単に剛性が高いのみでは実用性がない。例えば、ネジ締め等を考えると、穴の位置及びロボットの位置が完全に許容範囲に入っていれば問題ないが、一般にこのようなことは望めない。従って、組立ロボット等では一定の柔軟性が要求される。

このような目的のために、剛性を任意に変更する方法として、サ一ボ系に力を考慮したコンプライアンス制御がある。

しかし、このサーボ系を一般の線形制御（ P I制御）等によって実現しょうとすると制御対象の特性の変化などにより、設定したコンプライアンスを実現することが難しくなる。すなわち、一般の線形制御では、制御対象のパラメータが完全に既知でパラメータ変動がないとしてサーボ系を組んでいるので、大きなパラメータ変動のある制御対象に適用することが困難であった。発明の開示

本発明はこのような点に鑑みてなされたものであり、パラメータ変動 ·外乱等に強いスライディングモード制御によつてコンプライアンス制御を実現するコンプライアンス制御方式を提供することを目的とする。

本発明では上記課題を解決するために、

サ一ボ制御系を制御するコンプライアンス制御方式において、前記サーボ制御系をスライディングモード制御し、前記スライディングモード制御の切り換え面を位置、速度及び力の偏差によって切り換えることにより、任意のコンプライアンスに設定可能なサーボ制御系を形成することを特徴とするコンプライアンス制御方式が、提供される。

スライディングモード制御することにより制御対象のパラメ一タ変動があつても常に希望したコンプライアンスが得ら

4 る o 図面の簡単な説明第 1図はスライディングモード制御の処理のフローチヤ— 卜、

第 2図は本発明を実施するための口ボットシステムのハ一ドウユアの構成図である。発明を実施するための最良の形態以下、本発明の一実施例を図面に基づいて説明する。

第 2図は本発明を実施するためのロボットシステムのハードゥヱァの構成図である。ホストプロセッサ 9はロボット全体を制御するプロセッサである。ホストプロセッサ 9からはロボットの位置指令が共有 R A M 1 0に書き込まれる。なお、ホストプロセッサ 9 に結合される R O M、 R A M等は省略し "ある o

ロボットに内蔵されたサーボモータ 2 2を制御する D S P (ディジタル ' シグナル * プロセッサ） 1 1 は R 0 M 1 2 のシステムプログラムに従って、共有 R A M I 0の位置指令 rを一定時間ごとに読み取る。

D S P 1 1 は、この位置指令 r とサーボモータ 2 2に内蔵されたパルスコーダ 2 3からの位置フィ一ドバックとの差分による位置偏差 εを求める。また、この位置偏差 εを微分して、速度偏差 ε ^{( 1 )} を計算する。さらに、教示者によつて任意に与えられるトルク指令値 T c と図示されていないセンサからのフィ一ドノックトルク T f を求め、トルク偏差 ( T c - T f ) を計算する。これらのデータから後述するようにスライディングモード制御を実行する。

また、スライディングモード制御によって求められる電流補償ループの入力トルク Tを計算し、これからサーボモータ 2 2を駆動するための P W M波形を生成して、 D S L 1 4経由でサーボアンプ 2 1 に送る。

サーボアンプ 2 1 は P W M指令を受けて、サーボモータ 2 2を駆動する。サーボモータ 2 2は減速機を介して、アーム 2 6を駆動する。

次に口ボットのモデルとして第 2図を例にスライディングモ一ド制御を説明する。

T c ：ロボットか物体と接触して動作する場合の押しつけトルク、すなわち指令トルク

T f ：押しつけ力のフィードバックトルク（センサにより測定）

Θ ：ロホット tiifc

e ：位置偏差

ε ^{( , )} ：速度偏差

するとプラント方程式は以下のように表すことができる。 J * <9 ^{C 2 )} + T f = T

また、スライディングモード制御の切り換え関数 Sは、以下の式とする。

S = _ε い） + C * ε

+ ( 1 Κ ) * ( T c 一 T f )

とする。ここで、入力トルクを以下の様に置く、

T = A * S + T 1

ただし、 T 1 ：切り換え入力

A * S ：線形入力

である。ここで次の入力を与えると t (時間）が∞にて、必ず Sが 0に到達することが可能になる（証明は後述する）。ただし、

A— m a κ

と。 Jma n ：プラントの予想される最大ィナーシャ

Jm i n ：プラン卜の予想される最少ィナーシャ

である

次に、

T 1 -K 1 ( ε ) + Κ 2 (T c, T f )

+ K 3 (T f (') ) + Κ 4 ( r ⁽²⁾ )

とする。（ただし、 rは位置の指令値とする, )

ここからは、通常のスライディングモード制御と同じょう

S≥ 0の時、（切り換え入力て 1の選択する）

K 1 ( ε ) の項に関して

ε≥0 の時

Κ 1 ( ε ) = — C ² * J_min * e

ε < 0 の時

K 2 (T c， T f ) の項に関して

T c - T f ≥ 0の時

K 2 (T c， T f ) =— J_min * C * ( 1 /K)

氺（T c一 T f ) + T f

T c— T f < 0の時

K 2 (T c， T f ) =— J_ma« C * ( 1 ZK)

* (T c - T f ) + T f

K 3 (T f (') ) の項に関して

T f ( ¹ ) ≥ 0の時

K 3 (T f ⁽ⁿ ) =一 J_mi„ 氺（ 1 ZK) * T f ⁽¹⁾

T f ^Π) く 0の時

Κ 3 (T f ⁽¹⁾ ) =一 J_max * ( 1 ZK)

* T f ^(,}

K 4 ( 0 τ ^ί2 ) の項に関して、

Θ r ⁽²⁾ ≥ 0の時

Κ 4 ( (2 )

Θ r ⁽²⁾ < 0の時

K 4 ( r (²) ) = J_mi„ * 0 (2) S < 0の時（切り換え入力て 2の選択）

K 1 ( ε ) の項に関して

ε ≥0 の時

Κ 1 ( ε ) = - C ² * J_mi 氺 ε

e < 0 の時

K 1 ( ε ) = - C ² * J_m * ε

K 2 (Τ c， Τ f ) の項に関して

T c - T f ≥ 0の時

K 2 (T c， T f )

=- J_ma K * C * ( 1 ZK) * (T c Τ f )

+ T f

T c - T f < 0の時

K 2 (T c , Τ f )

=一 Jm i n * C * ( 1 ZK) * (T c Τ f ) + T f

K 3 (T f ^cn ) の項に関して、 T f ) ≥ 0の時

Κ 3 (T f (') ) 一 J_ma« * ( 1 ZK)

氺 T f (')

T f ( ' ) < 0の時

K 3 (T f (') ) 一 J_{m i}n * ( 1 ZK)

* T f (')

K 4 ( r (²⁾ ) の項に関して

Θ r ⁽²⁾ ≥ 0の時

K 4 ( r (²) ) = J _{mi n} * r (²⁾

Θ r ⁽²⁾ く 0の時

Κ 4 ( r (²) ) = J _maK * <9 r ⁽²

し、

S = 0

に収束した状態を考える。すなわち、コンプライアンスがあることの証明を考える。すなわち、

ε (') + C * ε + ( 1 ZK) * (T c一 T f ) = 0 であり、物体に接触している時を考えると、

T f = T c + K * ( ε (') + C * e )

となる。すなわち、

Kが小さければ T f ^ T cの力の制御が、

Kが大きければ T f K * ( ε ^(Ι + C * e ) のバネ性の強い制御が行える（K * Cがバネ定数、 Cが時定数）。

ロボッ卜が対象に接触していない場合は、

一 T c =K * ( ε ⁽¹) + C * ε )

を満足する位置で約り合って安定する。 T c - 0

なら、

e (') + C氺 ε = 0

の力を制御しない位置の制御が行える。

次に S uf = 0に収束する証明について述べる。

S = ε ^CI) + C * ε

+ ( 1 ZK) 水 (T c -T f )

( 1 )

J * <9 ⁽²⁾ + Τ f =Τ ( 2 )

(ブラント運動方程式）

ただし、

J ：ロボットのイナ一シャ（サーボモータのロータを含む） T ：入力トルク

：ロボットの位置

Θ r ⁽²⁾ - θ ⁽²⁾ = ε (²⁾ ( 3 )

ここで、リァプノフ闋数として、

V = ( S ² Ζ 2 ) をとる。

T = A * S +Τ 1 ( 4 )

(T 1 ：切り換え入力）

ここで、（ 1 ) 式の両辺を微分すると、

S ( 1 ) (2) + C氺 ε い）

一 ( 1 ΧΚ) 木 T f (')

( 5 )

( 2 ) 式に（ 3 ) 式を代入して整理すると、

(2) 0 (2 ) + (T f / J ) 一（TZ J ) ( 6 )

( 6 ) 式を（ 4 ) 、（ 1 ) 式により変形すると、

ε ( 2 ) = 0 τ + ( T f X J ) 一 ( 1 X J )

氺 {Α * ε ⁽¹⁾ + A * C 氺 ε

+ Α氺（ 1 /Κ) 氺（T c 一 T f )

+ T 1 } ( 7 )

( 7 ) 式を（ 5 ) 式に代入して整理すると、

S (') = {C - (A/ J ) } * ε (')

- (A * CZ J ) * ε + (9 r ^ί2)

+ (T f / J )

一 {A/ ( J * K) } * (T c 一 T f ) 一 (T 1 / J ) 一（ 1 ZK) * T f ('〕

( 8 )

( 1 ) 式を ε ⁽¹⁾ について解き、（ 8 ) 式に代入して整理すると、

S ^{(, )} = {C一（A/ J ) } * S - C ² * ε

- C * ( 1 ZK) * (T c 一 T f )

+ _Γ r ⁽²⁾ + (T f J )

一 (T 1 X J ) 一（ 1 ZK) * T f ⁽ⁿ

ここで、 S ⁾ を S倍すると、

S * S

= {C - (λ/ ) } * S ²

一 { C ²* ε + C * ( 1 ZK) 氺（T c 一 T f )

- (9 r ⁽² 一（T f Z J ) + (T 1 X J )

+ ( 1 XK) * T f ('〕 } * S ( 9 )

ここで、 S * S ⁽¹⁾ く 0を常に成立させるような入力を作る ( V ^(,) = S * S ^(,) < 0より極小値 0に V、 Sが収束する）。

C一（ AZ J ) < 0

A.— J m a. κ

とすると、

{C - (AX J ) } * S ² < 0

になる。ここで、（ 9 ) 式の右辺第 2項を負にすれば、

V ^(,) = S * S ⁽ⁿ < 0

が常に成立する。従って、（ 9 ) 式の右辺第 2項が負になるように T 1を選択すれば、 Sは 0に収束する。

第 1図はスライディングモード制御の処理のフローチャートである。図において、 S Pに続く数値はステップ審号を示す。

〔 S P 1〕共有 R AM 1 0より、位置指令 ^ r、トルク指令 T cを読み取る。

C S Ρ 2 ] D S Ρ 1 1の内部レジスタから、ロボットの位置 Θ フィードバックトルク T f を読み取る。

〔 S P 3〕上記のデータから、位置偏差 ε、速度偏差 ε ⁽¹⁾ 、トルク偏差（T c一 T f ) を計算する。

〔 S P 4〕先に述べた、切り換え面 Sを以下の式から求める。 S = e ⁽ⁿ + C * ε + ( 1 Κ) * (T c一 T f )

C S P 5 ) S≥ 0なら S P 6へ、そうでなければ S P ？へ進む。

〔 S P 6〕切り換え入力て 1を選択し、それぞれの K 1 ( ε ) 、 Κ 2 (T c , T f ) 、 K 3 (T f (') ) 、 K 4 ( Θ r (²⁾ ) の値を求め、切り換え入力 T 1を求める。

Τ 1 =Κ 1 ( e ) + K 2 (T c , T f )

+ K 3 (T f ^c,) ) + K 4 ( r (²) )

〔 S Ρ 7〕切り換え入力て 2を選択し、 S P 6 と同様に切り換え入力 Τ 1を求める。

〔 S P 8〕サーボアンプ 2 1への入力 Tを以下の式から求める o

T = A氺 S + T 1

C S P 9 ] 電流補償ループへの入力の受け渡しを行う。

このようにして、スライディングモード制御によるコンプライアンス制御を行うことにより、ロボットの姿勢によって、イナーシャ等が変化しても安定したコンプライアンス制御が可能になる。

上記の説明では口ボットのコンプライアンス制御方式について説明したが、他のィナーシャ等が変動するサーボ系の制御にも適用することができる。

以上説明したように本発明では、イナーシャ変動の大きな制御対象にスライディングモード制御を応用したコンプラィアンス制御を行うことより、たとえイナ一シャ等変動がしてもコンプラィァンスを設定した値に保てる口バストな系が得られる。

Claims

請求の範囲

1. サーボ制御系を制御するコンプライアンス制御方式において、

前記サーボ制御系をスライディングモード制御し、前記スライディングモ一ド制御の切り換え面を位置、速度及び力の偏差によって切り換えることにより、任意のコンプラィアンスに設定可能なサーボ制御系を形成することを特徵とするコンプライアンス制御方式。

2. 前記切り換え面を以下の式、

Sを切り換え面の値、 ε ⁽¹⁾ を速度偏差、 εを位置偏差、 T cをトルク指令値、 T f を各関節でセンサ等により測定されるフィードバックトルクとし、

S = ε ^(I) + C * ε + ( 1 ZK) * (T c一 T f )

で行うことを特徴とする請求項 1記載のコンプライアンス制御方式。

3. 前記サーボ制御系はロボットのサーボ制御系であることを特徴とする請求項 1記載のコンプライアンス制御方式。

4. 前記スライディングモード制御はロボットのサ一ボ制御系を制御するディジタル - シグナル ' プロセッサによって制御することを特徴とする請求項 1記載のコンプライアンス制御方式。