WO1991018332A1

WO1991018332A1 - Observer control system

Info

Publication number: WO1991018332A1
Application number: PCT/JP1991/000583
Authority: WO
Inventors: Nobutoshi Torii; Ryo Nihei; Tetsuaki Kato
Original assignee: Fanuc Ltd
Priority date: 1990-05-15
Filing date: 1991-04-26
Publication date: 1991-11-28
Also published as: JPH0424701A; EP0483367A1; EP0483367A4

Description

明細書 . ォブザーバ制御方式技術分野

本発明は機械系を制御するォブザーバ制御方式に関し、特にロボットの機構部を制御するォブザーバ制御方式に関する, 背景技術

ロボットの機構部のような剛性の低い機械系で通常のフィ一ドバック制御あるいはフィードフォワード制御では高速な制御は困難である。機構部の先端にフィ一ドバック用のセンサを取りつけることができれば制御は相当改善されるが、機構的にアームの先端等にセンサを設けるのは困難である。このためにオブザーバを使用して、機構部の各種の位置、速度、加速度等を推定して、すなわち推定状態変数を求め、これで系を制御することにより、より高速で安定した制御を可能にしている。

しかし、ロボッ卜の機構部等の制御では、オブザーバの入力に、重力の影響による重力項等の非線形項が推定状態変数の上に現れ、オフセットが生じ、オブザーバの特性が著しく低下し、状態フィ一ドバック制御を行うときに、状態フィ一ドバックゲインを大きく取れないという問題が生じている。発明の開示本発明はこのような点に鑑みてなされたものであり、メィンオブザーバへの入力非線形項を削除する外乱推定才ブザ— バを有するォブザーバ制御方式を提供することを目的とする本発明では上記課題を解決するために、

機械系を監視して、状態変数を推定して前記機械系を制御するォブザーバ制御方式において、前記機械系の線形要素に関する状態変数を推定するメィンオブザーバと、前記機械系の非線形要素に関する状態変数を推定する外乱推定ォブザーバと、前記メィンオブザーバの入力から前記非線形要素を取り除く加算器と、を有することを特徴とするォブザバ制御方式が、提供される。

外乱推定オブザーバにより、非線形の状態変数を推定し、この非線形の状態変数を、すなわち非線形項をメィンォブザーバから削除する。メィンオブザーバは線形項のみで、推定状態変数を推定できるので、非線形項の影響のない推定状態変数を推定できる。この推定状態変数によって、機械系を制御すれば、機械系の状態フィ一ドバックゲインを上げることができる。図面の簡単な説明第 1図は本発明のオブザーバ制御方式の一実施例のプロック図、

第 2図は本発明を実施するための口ボットシステムのハードウエアの構成図、

第 3図はメィンォブザーバのブ口ック図、第 4図は外乱推定オブザーバのブロック図、

第 5図は第 4図のブロック図の状態方程式を表す図である。発明を実施するための最良の形態以下、本発明の一実施例を図面に基づいて説明する。

第 1図は本発明のオブザーバ制御方式の一実施例のプロック図である。指令入力 Uは加算器 1 に入力され、フィ一ドバック量 Yを引き、加算器 1 の出力は補償回路 2に送られる。ここで、指令入力はトルク指令入力とする。補償回路 2 は P I制御回路等が使用され、その出力は加算器 3に入力され、 '· メインォブザーバ 7からの状態推定変数であるフィ一ドバック量を減算して、機械系 A ( 4 a ) に送られる。機械系 A ( 4 a ) は線形な機械系 B ( 4 b ) と、重力項等の外乱 Dからなっているものとする。外乱 Dは仮想した加算器 5を通じて加算器 3の出力に加算されるものとする。

一方、加算器 3の出力は加算器 6で、外乱推定オブザーバ 8で求められた非線形項 D sを減算して、メィンオブザーバ 7に入力される。この非線形項 D s は機械系 A ( a ) に舍まれる外乱 Dとほぼ等しい。この結果、オブザーバ 7 には線形項のみが入力されることになる。ォブザーバ 7はこの入力と、機械系 B ( 4 b ) のフィードバック信号 Yから推定状態変数 X 1 、 X 2 、 X 3を推定する。推定状態変数 X 1等の詳細は後述する。

メインォブザーバ 7への非線形項 D s は含まれていないので、非線形項の影響が推定状態変数 X 1 、 X 2 、 X 3等の上に現れず、メィンオブザーバ 7の特性が向上し、状態フィ一ドバックゲインを高めることができる。なお、推定状態変数 X I 、 X 2 、 X 3は制御ループのフィードバック信号として加算器 3で補償回路 2の出力から減算される。

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

ロボットに内蔵されたサーボモータ 2 2を制御する D S P (ディジタル . シグナル . プロセッサ） 1 1 は R 0 M 1 2のシステムプログラムに従って、共有 R A M I 0の位置指令を一定時間ごとに読み取り、ディジタル ♦ サ一ボ · L S I ( D S L ) 1 4に電圧指令を出力する。なお、必要な計算等は R A M 1 3を使用して行う。

D S P 1 1 は、内部に位置指令とサーボモータ 2 2に内蔵されたパルスコーダ 2 3からの位置フィ一ドバックとの差分によるエラー量から速度指令を計算する。さらに、位置フィ一ドバックを微分して、速度フィ一ドバックを計算する。この速度フィ一ドバックと速度指令との差分からトルク指令を計舁する。

サーボアンプ 2 1 は電圧指令を受けて、サ一ボモータ 2 2 を駆動する。サ一ボモータ 2 2は減速機を介して、アーム 2 6を駆動する。図では、減速機等の機械系のバネ成分 2 4 (バネ定数 K c ) 、ダンピング成分 2 5 (ダンピング定数 B k ) を模式的に表している。

また、 D S P 1 1 は第 1図に示す補償回路 2 、メイン才ブザーバ 7、外乱推定ォブザーバ 8の機能をそれぞれ実行する。次にロボットのモデルとして第 2図を例に説明する。ここで、

負荷側の条件を以下の通りとする。

J t ：負荷イナーシャ（モータ側換算値）

Θ t ：負荷側角度（モータ側換算値）

減速器の定数を以下の通りとする。

B k ：粘性項

K c ：バネ定数

また、モータの定数を以下の通りとする。

J m ：ロータイナ一シャ

Θ m ：モータ角度

まず、状態遷移行列を作成する。モータ側に注目して運動方程式を立てると、以下の式が得られる。

T - J m * θ m ⁱ² + B k * ( θ m ^ί - Θ t ^{( 1 )} )

+ K c * { 0 m - Θ t ) ( 1 )

また、負荷側に注目して運動方程式を立てると、以下の式が得られる。

0 = J ΐ * <9 t ⁽² + B k 氺（ <9 t ^{( 1 )} - θ τη ^ι )

+ Κ c * ( d t - θ ) ( 2 )

Θ m - Θ t - ε t ( 3 a )

Θ m ^{( ,} - 6 t ^{( , )} = ε t ^{( 1 )} ( 3 b ) Θ m ⁽²⁾ - 0 t ^ί2 = ε t ^C2) ( 3 c )

ここで、（ 1 ) 式を J mで除し、（ 2 ) 式を J tで除し、それぞれ両辺を引くと、

(TX J m)- = ε t ⁽²⁾ + [ ( B k Z J m)

+ ( B k X J t ) 〕 ε t ^{( 1}

+ 〔（K c Z J m)

+ (K c / J t ) 〕 ε t

ε t ^C2) 一〔 ( B k / J m) + ( B k / J t )

ε t ^{( I }} + [ ( K c J m)

+ ( K c J t ) ε t '

+ (TX J m) ( 4 )

また、（ 1 ) 式を（9 m (²) について（ 3 a ) 、 '（ 3 b ) 、

( 3 c ) 式を代入すると、

m =— （ B k Z J m) ε t ^{( 1 )}

一 (K c / J m) ε t

+ (TX J m) … -- ( 5 )

一、; ε t ^{( , )} = X 1

ε t = X 2

Θ m ^ι = X 3

とすると、（ 4 ) 、（ 5 ) 式から、第 3図に示す（ 6 ) 式が求まる。ここで、観測可能な状態変数は X 3であり、

Υ = 〔 0 0 1 3 CX I X 2 X 3 ] ^τ

( Ί )

この状態方程式から一般的な技法である同一次元オブザーバ、すなわちメインオブザーバ 7を作成して、 X 1、. X 2、 X 3 を推定する。

次に外乱推定ォブザーバについて述べる。第 4図は外乱推定オブザーバのブロック図である。入力 U 1 (入力電流）はトルク定数 Κ ΐをかけることにより入力トルクになり、加算器 3 2に入力される。加算器 3 2で外乱トルク（X 5 ) を加算して、ブロック 3 3に入力する。ここで、 Jは機械系のィナーシャである。ブロック 3 3の出力は X 4 ^m であり、ブロック 3 4で積分されて出力 X 4 となる。ここで、 Sはラブラス演算子である。

第 5図は第 4図のブロック図の状態方程式を表す図である。 ( 8 a ) 式は第 4図のブロック図の状態方程式であり、（ 8 b) 、 ( 8 c ) 式はそれぞれ、（ 8 a ) 式の係数を示す式である。ここで、観測可能な状態変数は、

Y= 〔 l 0〕 [X 4 Χ 5〕 ^τ

で表される。これらの状態方程式からオブザーバを組む一般的手法により、 Χ 4、 X 5を推定する同一次元オブザーバ、すなわち外乱推定オブザーバ 8を組むことができる。

この外乱推定ォブザーバ 8を見ると、推定外乱 X 5はその微分 X 5 ⁽¹⁾ = 0であり、殆ど一定の定常外乱（変化の少ない、重力項、摩擦項）のみを推定することが判る。また、ォブザーバゲィンを決定する際、振動の成分が X 5 に影響しないように共振周波数よりもかなり低い立ち上がり周波数になるように設定する。この Χ 5をメインォブザーバ 7の入力から削除する、すなわち第 1図の加算器 6で減算する。これによって、外乱、すなわち非線形成分の悪影響を取り除くことが可能になる。従って、非線形項の影響を取り除いたため、状態フィードバックゲインを高く設定できるようになり、それにより全体の系を安定にすることができる。具体的には防振効果を上げることが可能になる。

上記の説明ではロボットの機構部を例に説明したが、それ以外の剛性の低い機械系の制御にも同様に適用できることはいうまでもない。

以上説明したように本発明では、非線形成分を推定する外乱推定オブザーバを設けて、外乱を推定し、メイン'ォブザーバの入力から削除するように構成したので、メィン才ブザ一バの推定状態変数の上に非線形項の影響が現れず、状態フィ一ドバックゲインを上げることができ、全体の系を安定にすることができる。さらに、振動も減少する。

Claims

請求の範囲

1 . 機械系を監視して、状態変数を推定して前記機械系を制御するォブザーバ制御方式において、

前記機械系の線形要素に関する状態変数を推定するメィンォブザーノと、

前記機械形の非線形要素に関する状態変数を推定する外乱推定オブザーバと、

前記メインォブザーバの入力から前記非線形要素を取り除く加算器と、 - を有することを特徴とするオブザーバ制御方^;。 '

2 . 前記機械系はロボットの機構部であることを特徵とする請求項 1記載のオブザーバ制御方式。

3 . 前記非線形項は少なくとも重力による非線形項を含むことを特徴とする請求項 1記載のオブザーバ制御方式。

4 . 前記外乱推定オブザーバの立ち上がり周波数として、前記メィンオブザーバで必要とする推定状態変数の周波数に影響を与えない周波数を設定したことを特徴とする請求項 1 記載のオブザーバ制御方式。