JPS6368903A - デイジタルサ−ボ制御装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔概　要〕 本発明は、振動しやすい低剛性のアームを・す“−ボ制
御するディジタルサーボ制御装置において、アーム全体
を一つのオブザーバ（状態推定器）で推定すると制御系
が不安定になりやすいので、アームの根元の状態のみ推
定する第１のオブザーバと振動状態を推定する第２のオ
ブザーバとにオブザーバを分離し、２つのオブザーバの
推定結果に基づいてアーム全体をサーボ制御するように
して、制御系の安定性を高めるようにしたものである。

〔産業上の利用分野〕

本発明は産業用ロボットのサーボ制御装置に関する。

〔従来の技術〕

作業時間を短縮するためには、ロボットを高速に動かず
必要がある。そこで高速動作を目的としてアームの軽量
化を図ると、アームの剛性が不足し振動しやすくなる。

ロボットの加速度を上げれば更に振動しやすくなる。ア
ームが振動すると、正しい位置制御、速度制御ができな
い。従って、サーボ制御装置には振動抑制機能を必要と
する。

振動抑制を行わない場合は、アームの根元の位置のみを
観測し、アームの位置制御、速度制御等のサーボ制御を
行なっていたが、アームの剛性が不足してアームの振動
が無視出来ない場合には、アームの根元の位置のみでな
く、アームの振動状態を観測して、振動を抑制しながら
サーボ制御を行う必要がある。

振動を観測する手段として、アームの撓み量を検出する
方法、或いはアームの先端の速度、加速度等を検出する
方法がある。そこで、アームの根元の目標値からの偏差
と撓み量、或いは根元の目標値からの偏差と先端の速度
、加速度等をフィードバックして、振動を抑制しながら
アームのサーボ制御を行えばよいが、フィードバンクす
る量が複数種あるので、それぞれどの程度フィードバン
クするかを示すフィードバック定数の決定が困難である
。

更に、アームの位置の定常偏差を除去するために、位置
偏差を積分してフィードバックするようにすると、各フ
ィードバック定数を決めるのがより困難になる。

そこで、アームの状態方程式を立てて、極配置法、或い
は最適レギュレータ理論等を用いて状態フィードバック
定数を決定し、状態フィードバックを行うことでサーボ
制御を行うようにすれば、フィードバック定数を決める
のが比較的簡単になる。

状態フィードバンクを行うためには、アームの総ての状
態が判らなければいけないが、実際には、根元の位置と
先端の加速度というように、一部の状態しか判らないの
で、状態推定器（オブザーバ）を用いて、各状態を推定
する必要がある。

このような方法で振動抑制を行いながら、アームのサー
ボ制御を行う方式が、アナログ制御系についてはかねて
より種々提唱されている（例えば特開昭５８−３００１
）。

近年、制御装置の小型化、低価格化、高集積化。

高機能化を図って、アナログ制御系からディジタル制御
系に置き換えることが有効であり、振動抑制機能を付け
たサーボ制御装置もディジタル制御系にすることが有効
である。

この場合、アーム全体は一つの連続時間系であるので、
アームの各状態を推定するためには、アームの全状態を
推定する一つのオブザーバをディジタル制御系で構成す
ることになる。

第４図は従来のサーボ制御を説明するための図で、１は
アーム、２はモータ、３はサーボ制御装置、４はオブザ
ーバである。

上記アーム１全体の状態方程式を立てる場合、アーム１
の振動には無限価の振動モードがあるので、そのうち影
響の大きい基本的なモードのみを用いて状態方程式を立
てる。そのために、振動を表現する方程式の部分の低次
元化による誤差は、振動を表現しない部分に比べて大き
い。また、振動を観測するための撓み量を計る歪ゲージ
や、先端の加速度を計る加速度計は、アームの根元の位
置を計るエンコーダに比べて測定誤差が大きいのが一般
的である。

〔発明が解決しようとする問題点〕

このような場合に、アーム１全体を一つのオブザーバ４
で推定しようとすると、オブザーバ４の入力は操作量と
振動の観測量になるので、根元の位置、速度等の状態推
定にも振動の観測量が使用される。ところが、振動の観
測量は、上記したように根元の位置に比べて不正確であ
るので、根元の状態推定も不正確になる。そのために根
元の状態による状態フィードバック定数を大きくするこ
とができなくなり、アーム１全体の追従特性が劣る。ま
た、このフィードバック定数を大きくすると、発振しや
す（なるという問題がある。

本発明の目的は、アームの追従速度を上げ、しかも制御
系全体の安定性を損なうことのないディジタルサーボ制
御装置を提供することにある。

〔問題点を解決するための手段〕

第１図は本発明の詳細な説明するための歯で、１はアー
ム、２はこのアーム１を駆動するモータ、３はサーボ制
御装置、４は上記アーム１の根元の位置を示す信号に基
づいてアーム１の根元の状態を推定する第１のオブザー
バ、５はアーム１の先端の位置、速度または加速度のう
ちの一つとアーム１の根元の位置とからアームＩの振動
の状態を推定する第２のオブザーバである。

上記アーム１の根元の状態推定は、信号が振動の観測量
に比べて正確な根元の位置信号のみによって行い、７−
ム１の振動の状態推定には、アームの根元の位置と振動
の観測量によって行うようにした。

〔作　用〕

振動の観測量が不正確であっても、アームの根元と振動
を別々のオブザーバで状態推定することにより、アーム
の根元の状態は正しく推定できるので、アームの根元の
状態による状態フィードバック定数を大きくすることで
、アーム全体の追従特性を上げることが可能となり、ま
たこれにより、振動の観測量の不正確さにより振動抑制
効果が低下しても、制御系全体が不安定になることは防
止される。

〔実　施　例〕

以下本発明の一実施例を、第２図及び第３図により説明
する。第２図は上記一実施例の構成を示す図、第３図は
そのブロック図である。

本実施例は、第２図に見られるように、ＤＣ（直流）モ
ータ２．減速器１１により駆動される低剛性のアーム１
を、アーム１の先端に加速度計１０を付けて振動抑制を
行いながら、アーム１の根元の位置をエンコーダ１２で
観測してサーボ制御する。

制御系はＣＰＵ１９とソフトウェアで構成される。

その制御式は後述する。なお、１３は加速度系アンプ、
１４はＡ／Ｄコンバータ、１５はパルス発生回路、１６
はカウンタ、１７はパワーアンプ、１８はＤ／Ａコンバ
ータである。

更に本実施例では第３図に見られる如く、アーム１の根
元の定常誤差を除去するための積分器と、アームの根元
及び先端の目標位置、目標加速度を発生する関数発生器
を設けた。

まずアームの制御を行うための状態方程式を説明する。

アーム１の根元の速度ＸＩ＋位置Ｘ２　＋　アーム先端
の速度Ｘｆｆ＋位置ｘ４を状態変数として選び、モータ
に流す電流Ｕを操作入力、アームの根元の位置ｙ１．先
端の加速度ｙ。を系の出力とすると、連続時間系の状態
方程式は、以下のようになる。

−−−−−−−−−−−−−−■ 但し、Ｋ１はアームの根元の操作量に対する位置出力の
ゲイン、Ｋｅはアームの根元の位置に対する先端の加速
度出力のゲイン、ω０はアームの振動角周波数、ζはア
ームの振動ダンピング定数である。

上記０式中のＫｍ　、Ｋ　ｅ　＋　ωｅ、ζをサンプリ
ング時間Ｔで規格化（時間の単位をサンプリングクロッ
ク　ＣＣ）とすること）すると、Ｋ、　＝２４４．１　
　（１／Ｓ２）　Ｘｌ０−”　（Ｓ”／Ｃ２）−〇、０
００２４１１　　（１／Ｃ２）Ｋｅ　＝０．００５０５
５　（Ｓ２）　ｘ１０６ｒＣ”／５２）−５０５５［Ｃ
２］ ω６−２πｘ６．４　Ｘ　（ｒａｄ／Ｓ　）　Ｘｌ０３
（Ｓ／Ｃ〕＝０．０４０２１６　（ｒａｄ／Ｃ〕 ζ　−０，０３２２ 次にアームの根元と別々に状態推定した場合の制御につ
いて述べる。

アームの根元と先端を別々に、漸近的オブザーバ（有限
推定を行わないオブザーバ）を用いて状態推定する方法
は、下記のようになる。連続時間系の状態方程式０式を
、根元と先端に分離すると、＋Ｋｏ　ωθ’ｙ、（ｔ） ＋８°１７３′・３゛ゝ　　　　　−１２，−４−１゜
となる。それぞれの系をサンプリング時間Ｔ＝１　　（
Ｃ） で離散時間化する。本来、２つの系は一つの連続時間の
中で動作しているので、別々に離散時間化するのは正し
くない。しかし、近似的には根元の出力ｙ、（ｔ）にサ
ンプラを通し、その結果を０次ホールドして先端の系の
入力としてもよいと考えられる。

■、■式を別々に離散時間化すると、 ｘ、（ｋｌ１）＝Ａ＋＋＋　ｘ、＋ｂｌＩｕＴｋ）ｙ　
ｍ　（ｋｌ　＝　ｃ　ｍ　ｘ　ｍ　（ｋ）Ｘｅ　　（ｋ
ｌ１）　＝Ａｅ　ｘｅ　（ｋ）＋ｂｅ　Ｙｌｌｌ（ｋ１
ｙｅ　（ｋ）＝ｃｅ　　Ｘｅ　（ｋ）＋　ｄｅ　　ｙｍ
　（ｋｌ但し、（ｋ）はｋＴ時刻での値を示す。

なお、上式中、太字の英文字（Ａ−、Ａｏ）は行列を示
し、太字の英小文字（×□、ｂ、、ｃ、。

Ｘｅ　＋　ｔ）　＋１　＋　　ｃ、）はベクトルを示す
。また、細字の英小文字（ｘｌ　＋　　Ｘ２　＋　　ｘ
、、　ｌ　　Ｘａ　＋　　Ｙｌｌｌｙｅ、ｄｅ）はスカ
ラーを示す。以下同様の使い方をする。

■、■式の系について漸近的オブザーバを構成し、オブ
ザーバゲインｈ、、ｈ６を求める。状態フィードバック
ゲインｆヶは、根元と先端を一つの系として最適レギュ
レータ問題を解いて求めたｆ、を（ｆｌＩ　ｆｏ　　ｆ
ｓ）として用いる。

以上の方式による制御式は、下記のようになる。

”ｉ　ｍ　（ｋｌ　＝　ｙ　ｍ伽）−ｒ、（ト））”；
ｅ　（ｋ）−Ｖ　ｅ　（ｋ）　　ｒ　ｅ　（ｋ）マ、（
ｋｌ＝れ伽）＋ｈｍ　　＜ｙ、ｔｓ＋＋−１ｓ（ト））
）ｘｅ＝ｘｅｆｋ）＋ ｈｅ　　（？ｅ（ｋ）　　ｄ。Ｖｍ　（ｋｌ　　７３　
（ｋｌ）ｗ（ｋ）＝ｗ　（ｋ　−１）　＋　Ｙ、　（ｋ
）ｕＣｋ）＝　　ｆ　ｍ　”；Ｃｍ　（ｋ）　　ｆ　ｅ
　Ｘｅ　（ｋ）　　ｆ　＠　ｗ（ｋ）ｕ　（ｋ）　＝　
ｕ　（ｋｌ　＋　ｕ　ｍ　（ｋ）ｘｍ　　（ｋｌ１）＝
Ａ、マ、（ト））＋ｂ、τ（ｋ）Ｘｅ　　（ｋｌ　１）
　−Ａｅ　Ｘｅ　（ｋｌ＋ｂｅ　”’；ｖｈ　（ｋ）ｙ
　ｍ　　（ｋ　＋１　）　＝ｃ　ｍれ　（ｋｌｌ）糺　
（ｋｌ１）　−ｃｅ　Ｘｅ　　（ｋｌ１）ｒ、（ｋｌ１
）＝に＠　ａｌｌｌ　（ｋｌ１）但し、ｒｍ　＋　　ｒ
　Ｏ＋　　ａｌｈ　＋　　ｖｌｌは、それぞれ関数発生
器によって作り出された根元の目標位置、先端の目標加
速度、根元の目標加速度、根元の目標速度であり、Ｙｍ
＋Ｙｅはそれぞれ、目標位置からの位置偏差、目標加速
度からの加速度偏差である。また、ｕ、、ｕはそれぞれ
、目（票状態にするだめのフィードフォワード量、目標
状態からのずれを零にするためのフィードバンク量であ
る。更に、ｘ、、ｘθは、状態変数Ｘ　ｍ　＋　　Ｘ　
Ｏの偏差系における表現、Ｘ、、Ｘθ、：ｙ□、Ｌは、
オブザーバによる推定値、Ｗは積分量である。

第３図は上記制御式に基づいて構成したサーボ制御装置
を示すブロック図である。

以上述べた本実施例によれば、アーム１の根元の状態推
定は、信号が振動の観測量に比べて正確な根元の位置信
号によって行い、振動の状態推定はアーム１の根元の位
置とアーム１の振動の観測量とによって行うので、振動
が抑制されるとともにアーム１全体の追従特性が向上す
る。

〔発明の効果〕

以上説明した如く、本発明によれば、アームの振動を効
果的に抑制しながら追従特性を向上することが可能とな
り、更に、制御系全体が不安定になることが防止される
。

【図面の簡単な説明】

第１図は本発明の原理説明図、 第２図は本発明一実施例の構成説明図、第３図は上記一
実施例のブロック図、 第４図は従来のサーボ制御説明図である。 図において、１はアーム、２はＤＣモータ、３はサーボ
制御装置、４及び５はそれぞれ第１及び第２のオブザー
バ、１０は加速度系、１２はエンコーダを示す。 第１図 第　　４　　図 第　　２　　図

Claims (1)

【特許請求の範囲】
1. アームの振動を抑制しながら、アームの位置或いは速度
   制御を行うディジタルサーボ制御装置において、アーム
   の根元の位置に基づいてアームの根元の状態を推定する
   第１のオブザーバと、アームの先端の位置、速度及び加
   速度のうちの少なくとも一つとアームの根元の位置とに
   よってアームの振動の状態を推定する第２のオブザーバ
   を具え、前記第１及び第２のオブザーバの推定結果に基
   づいてアーム全体をサーボ制御するようにしたことを特
   徴とするディジタルサーボ制御装置。