WO1991009357A1

WO1991009357A1 - Sliding mode control system including feedback of amount of twist

Info

Publication number: WO1991009357A1
Application number: PCT/JP1990/001605
Authority: WO
Inventors: Nobutoshi Torii; Ryo Nihei; Tetsuaki Kato
Original assignee: Fanuc Ltd
Priority date: 1989-12-11
Filing date: 1990-12-07
Publication date: 1991-06-27
Also published as: KR920701888A; JPH03180903A; DE69023495T2; DE69023495D1; EP0471077A1; EP0471077A4; KR950009528B1; EP0471077B1

Description

明細書ねじれ量のフィードバックを合む

スライディングモード制御方式

技術分野

本発明はロボットのねじれ量のフィ一ドノックを舍むスラィディングモード制御方式に関し、特にねじれ量をフィルタした値により切り換え面を変更することにより振動を低減したねじれ量のフィ一ドバックを含むスライディングモード制御方式に関する。背景技術

ロボッ卜のようなアームの伸縮状態によつて、負荷ィナ一シャが大幅に変化し、かつサーボモータと機構部の減速比が大きい機械稼働部を有する系では通常のフィードバック制御のみでは応答が遅く、実用に供しえない。

このために、ロボットのアーム等の機械稼働部のパネ系のパネ定数及び粘性項を種々の姿勢で測定し、オンラインで従来の線型制御の方法によりダイナミックに変化するフィードフォア一ド項を計算して、フィードフォア一ド制御を行う。このような、ロボッ卜のイナ一シャ変動に対応するために、イナーシャ変動に応じてトルク指令値を変えるスライディングモ— ド制御方式として、本出願人による特願平 1 一 1 1 7 5 1 8号がある。これを改良するものとして、スライディングモード制御の切り換え面をサーボモータの位置とサーボモータで駆動される機械稼働部の位置の差であるねじれと、ねじれ速度によつて変更して、スライディングモード制御を行う方式として、本出願人による特願平 1 一 2 5 3 7 6 7号がある。

しかし、上記の方式では位置ループゲインを共振振動周波数より小さく設定する必要があり、サーボ剛性が低下するという問題がある。発明の開示

本発明はこのような点に鑑みてなされたものであり、ねじれ量をフィルタ処理した値によって、切り換え面を変更することでスライディングモード制御することにより、剛性を高く維持して振動を低減したねじれ量のフィ一ドバックを舍むスライディングモ一ド制御方式を提供することを目的とする。本発明では上記課題を解決するために、

サ一ボループを制御するねじれ量のフィ一ドバックを舍むスライディングモード制御方式において、サ一ポモータの位置と、前記サーボモータで駆動される機械稼働部の位置の差であるねじれ量にフィルタ一をかけた値により切り換え面を変更するスライディングモ一ド制御を行うことを特徵とするねじれ量のフィ ― ドバックを舍むスライディングモ一ド制御方式が、提供される。

ねじれ量に対して、分子及び分母が 1次であるフィルタをかける。このフィルタによって、振動の少ない、イナーシャ変動に対してロバストな系が実現される。図面の簡単な説明第 1図は本発明のねじれ量のフィ一ドバックを含むスラィディングモ一ド制御方式のフローチャート、

第 2図は本発明の一実施例のロボッ卜のサーボ制御ループの例を示す図である。発明を実施するための最良の形態以下、本発明の一実施例を図面に基づいて説明する。

第 2図は本発明の一実施例の口ボットのサ—ボ制御ループの例を示す図である。ホスト C P U 1 はロボット制御装置全体を制御するプロセッサであり、システムプログラム用の R 〇 M、ワーク R A M等は省略してある。ホスト C P U 1からは位置指令値が共有メモリ 2に書き込まれる。サ一ポ用ディジタルサーボ回路 3は D S P (ディジタル · シグナル * プ口セッサ）を中心として構成されている。ディジタルサ一ボ回路 3は共有メモリから一定時間ごとに位置指令値を読み出し、さらに帰還信号レジスタ 4からモータ情報を読みだし、サーボ系の指令を計算し、ロボット 5に内蔵された各軸のサ一ボモータの位置、速度及び電流を制御する。以下に述べるスラィディングモ一ド制御の種々の計算もディジタルサーボ回路 3によって実行される。

第 1図は本発明のねじれ量のフィ―ドバックを含むスラィデイングモード制御方式のフローチャートである。図において、 S Pに続く数値はステップ番号を示す。

〔 S P 1〕共有メモリ 2から位置指令を読みだし、帰還信号レジスタ 4からフィードバックパルスを読み取る。

C S P 2 位置及び速度のエラー量を計算する。

〔 S P 3〕切り換え面 S ufの値を計算する。ただし、切り換え面 S ufを計算するときに、ねじれ量にフィルタをかけて計算する。その詳細は後述する。

〔 S P 4〕 S ufが正なら S P 5へ、そうでなければ S P 6へ進む。

C S P 5 S ufが正であるので、切り換え入力の 1を選択する。切り換え入力の詳細については後述する。

〔S P 6〕 S ufが正でないので、切り換え入力の 2を選択する。切り換え入力の詳細については後述する。

〔 S P 7〕電流補償ループへの入力を計算する。

〔S P 8〕電流補償ループへの入力の受け渡しを実行する。本発明の特徴は、スライディングモードの切り換え面にォブザーバにより推定したねじれ量に特殊なフィルタを掛けた値をフィードバックすることにより、バネ系に対してもイナ一シャ変動に適応性をもつた σバストな系を実現する。以下、その詳細を述べる。

切り換え面 S ufを計算する式を具体的に書くと、

S uf = ε ⁽¹ + C * e + D * j" ( e + C * e )

-K * { ( S + C) / ( S + A) } d - d t ) ここで、 { ( S + C) / (S + A) } は本発明で使用するフィルタを表す項であり、（ー t ) はねじれ量である。なお、切り換え面は一般に Sで表されるが、ここではラプラス変換の演算子との混同を避けるために S ufで表す。上記の式中では、各記号の意味は下記の通りである。

S uf ：切り換え面

5 ：ラプラス演算子

6 ：モータ位置

Θ r ：位置指令値

Θ t ：機械稼働部の先端位置

ε ：位置の遅れ量、θ 一 Θ、

次にロボットの制御を考える。 Jをボットのイナ一シャ、 Τを入力トルクとすると、

J * （2) =丁 ( 1 )

が成立する。簡単の為（ 1 ) 式において D= 0の状態で説明すると、

S uf = ε ^(η + C * ε - K * { ( S + C )

/ ( S + A ) } { Θ - Θ t ) ( 2 )

T = J o 氺。 * ε + J o * ω c * C 氺 ε ⁽¹⁾ + T 1

ここで、各記号は以下の量を表す。

Τ 1 ：切り換え入力

J 0 ：予想される最小イナーシャ

ω₀ ：後で決定される定数

ε η ： θ - θ \

ここで、次の入力を与えるとフィードフォア一ドを舍めて必ず切り換え面に到達することが可能になる。ここでは新たに加えた〔一 Κ * { ( S + C ) / ( S + A) } ε η〕の項について書く。他の項については省略する。

T 1 =K 1 ( ε ) + Κ 2 ( 0 τ ) + Κ 6 ( ε η )

+ Κ 7 ( ε η )

とすると、 Κ 6 ( ε η ) 、 Κ 7 ( e n ) が新たに加えたフィルタに相当する項であり、これらの項について述べる。その他の項は先の出願である特願平 1 一 2 5 3 7 6 7号に記載されている内容と同様であるので、その説明は省略する。

( a ) S uf ≥ 0の時（第 1図のステップ 5の切り換え入力 I の計算である。）

( a i ) K 6 ( ε η ) の項に関して、

ε η ≥ 0 の時、

Κ 6 ( ε II ) - 0

ε η < 0 の時、

Κ 6 ( s n ) = C * ( J o - J MAX ) * K

* { ( S + C ) / ( S + A ) } * e n

( a ii ) K 7 ( e n ) の項に関して、

ε η ^{( η} ≥ 0の時

Κ 7 ( ε η ^{C1 )} ) = - J ο * Κ * { ( S + C ) /

( S + A) } * ε η ^{(1 )}

ε η < 0の時

Κ 7 ( e η ^{C 1 )} ) = - J MAX * Κ * { ( S + C )

/ ( S + A) } ε n ^{( 1)}

( b ) S uiく 0の時（第 1図のステップ 6の切り換え入力 Π の計算である。）

( i ) K 6 ( ε τι ) の項に関して、 ε n≥ 0の時

K 6 ( ε n ) = C * ( J J _MAX ) * K

* { ( S + C) Z ( S + A) } * e n ε n < 0の時

K 6 ( e n ) = 0

( b ii ) K 7 ( e n ^CIJ ) の項に関して、

e n ⁽ⁿ ≥ 0の時、

K 7 ( e n ^C1) ) J MAX * K * { ( S + C) /

( S + A ) } * ε n ⁽¹⁾

e n ( I ) < 0

K 7 ( ε n ^c,) ) =一 J。 * K * { ( S + C ) /

( S + A ) } 本 ε n ( ¹ )

このように、 S ufの値に応じて、切り換え入力 T 1 を与えていくことにより、イナ一シャが変化しても、サ一ボ剛性を低下させることなく、振動の少ない制御が可能になる。

ここで、この ε η ε n ( ¹ ) がロボットのバネモデルに対して制振効果のあることを証明する。上述する入力により S ufが 0 になった時を考えると、

_{0 = ε} い） + C * ε -Κ * { ( S + C ) / ( S + A) }

* ε n ^cn

でめる。

ε = θ τ - θ

ε ⁽¹) = r ^{η - θ ^{( 1 )}

ε η : θ - θ t

を代入して、両辺をラプラス変換して整理すると、 ( S + C ) Θ て = ( S + C + K) ー K *

{ ( S + C ) / ( S + A) } * t

( 3 ) となる。

ここでバネ系を考え、減速器を中心としたバネ系の定数を、 B k ：ダンピング項

K c ：バネ係数

J t ：減速器から先の負荷ィナーシャ

とすると、

( t Z ）二（B k S + K c ) Z ( J t S ²

+ B k S + K c ) ( 4 )

(一般の口ボットはこの B kが小さくまた K cが小さい為、ダンピングが悪くしかも低い周波数で振動する。）

( 4 ) 式を ^について解き（ 3 ) 式に代入し整理すると、 ( t Z r ) = (B k S + K c ) Z

{ ( J t S ² + B k S + K c

+ K J t S ² X ( S + A) }

ここで Aを小さく取ると（具体的には上のバネ系の共振周波数で（ S + A) が Sと見なせるまで）、

{ 0 t / 6 τ ) = (B k S + K c ) / { J t S ²

+ (B k + K J t ) S + K c }

となり Kがダンピング項となり振動抑制効果のあることがわかる。

次に、切り換え入力により適応性が補償されることを証明する。以下の証明においては、 e n { ( S + C ) / ( S + A ) } * ε n

とおくことにより得られる。

S uf = £ ^{U )} + C 水 ε — K 氺 ε η

( 5 ) (切り換え面）

T = J ₀ 氺 w _c * ε ^{( 1} + J ο * ω c * C * ε + T 1

… ( 6 ) (入力）

ここで、上記の記号は以下の意味を有する。

Τ 1 ：切り換え入力

J 0 ：予想される最小イナ一シャ

ω c ：後で決定する定数

J * ^ ^C2) = T ( 7 ) (制御対象）

ε = (9 Γ— ^ とすると、

ε (1) ^ ^ _r en _ (9 en

(25 (2) (2)

θ θ ( 8 )

である。

θ _R ：位置の指令値

Θ ：位置のフィードノ、'ック

である。ここで、リアプノフ闋数候補として、

V = 1 κ 2 * s ²

を考える。（Vの最小値は 0であり、もし Vく 0であれば、

Vが単調減少である為、 Vは最小値 0に収束する。また、これにより Sは常に 0 に収束し応答性が S uf= 0の一定の応答関数によって決定される）

( 5 ) 式より、

S uf 〕 = ε ^C23 + C * ε ^{( 1 )} - Κ * ε η ^Π ( 9 ) ( 8 ) 式を（ 7 ) に代入して整理すると

ε (²) = Θ て ⁽²) - ΤΧ J ( 1 0 )

( 1 0 ) 式へ（ 6 ) 式を代入して整理すると、

ε ^C2) = r ^C2) - J o * ω c 氺 ε ⁽¹⁾ / J

— J。 * o _c * C * ε / J - T l / J

( 1 1 )

( 1 1 ) 式を（ 9 ) 式へ代入して整理すると、

S uf ^cn = (C - ω ο * J。 Z J ) * ε (') — J。 Z J

* ω c * C * e - K * ε η - T l / J + θ T ^ί2' ( 1 2 )

( 5 ) 式を ε い）について解くと、

ε ^c,) = S— C 氺 ε + Κ氺 ε η )

これを（ 1 2 ) 式に代入し整理すると、

S uf ^CI) = ( C - ω_ε * J ο J ) 氺 S _ C ² * e +

( C - J o / J * iu _c ) * K

* ε η -Κ * ε η ⁰⁾ -T 1 J + Θ r ⁽²⁾

V ^C1) =： S * S ^C1)

= (C - ω c * J o / J ) * S ² 一 [C ² * ε - ( C - J o / J * ω c ) * Κ * ε η + Κ氺 ε η ⁽¹) + Ύ 1 J - 0 τ ⁽² ] * S

今、 ω c = C * J MAX / J 0 とすると、

(C - oc * J o J ) * S ² く 0 となる。

これより常に、

V ^Cl) < 0

にするには、 ― [ C ² * ε - (C一 J。 ZJ * io_c ) * K

* ε n + K * ε n ^cn + T 1 / J - r ⁽²⁾ ] * S < 0 になるように切り換え入力 T lを決定し、振動部分だけを取り出すと先に述べた結果が得られる。

上記の説明ではロボットの機械稼働部を例に説明したが、サーボモータで駆動される他の機構にも同様に適用することができることはいうまでもない。

以上説明したように本発明では、ねじれ量に対して、フィルタリングを導入することにより、剛性を犠牲にすることなく、振動を減少しつつ、指令追従性のよいサーボ系が実現できる。これにより、軌跡精度が高く、定常速度時の振動及び停止時残留振動が小さなロボットが実現できる。

Claims

請求の範囲

1 . サ一ボル一プを制御するねじれ量のフィ一ドバックを含むスライディングモ― ド制御方式において、

サ一ボモータの位置と、前記サーボモータで駆動される機械稼働部の位置の差であるねじれ量にフィルタをかけた値により切り換え面を変更するスライディングモード制御を行うことを特徴とするねじれ量のフィードバックを含むスライディングモード制御方式。

2 . 前記フィルタは分子及び分母がラブラス演算子に関し一次であることを特徵とする特許請求の範囲第 1項記載のねじれ量のフイードバックを含むスライディングモード制御方式。

3 . 前記ねじれ量はオブザーバによつて推定することを特徴とする特許請求の範囲第 1項記載のねじれ量のフィ一ドバックを舍むスライディングモード制御方式。

4 . 前記スライディングモード制御によってロボットを制御することを特徴とする特許請求の範囲第 1項記載のねじれ量のフィ一ドバックを含むスライディングモード制御方式。

5 . 前記機械稼働部は前記ロボットのアームであることを特徵とする特許請求の範囲第 4項記載のねじれ量のフィードバックを舍むスライディングモード制御方式。