JP4860277B2 - 多慣性共振系の振動抑制制御方法及び装置 - Google Patents

Description

本発明は、多慣性共振系の振動抑制制御方法及び装置に係り、特に、フレキシブルマニピュレータやＸＹロボット等に用いられる、モータ制御装置の振動抑制制御に関する。

一般に、フレキシブルマニピュレータや、ボールねじやタイミングベルト等を用いたＸＹロボット等のモータ駆動系においては、図１に例示する如く、モータＭと負荷Ａが、剛性よりも軽量化を優先して低剛性の弾性軸（図ではプーリＰ間に掛け渡されたベルトＢ）で結合されていると共振系となり、軸ねじれ振動等が発生し問題となることがある。

実際の共振系は、多数の振動モードや固有周波数が存在するため、図２に例示する如く、多慣性共振系としてモデル化される。図２において、ＪｍはモータＭの慣性、Ｋｆ１、Ｋｆ２・・・は、ばね定数、Ｊａ１、Ｊａ２・・・Ｊａｎは負荷Ａの慣性である。

この多慣性共振系は、図３に示すようなブロック図で表わされる。図３において、θｍはモータＭの回転角度（モータ位置）、θａは負荷Ａの回転角度（負荷位置）、Ｔはトルク、ｓはラプラス演算子、添字ｍはモータ、添字ａは負荷、添字disは外乱、添字reacは軸ねじれ反力を表わす。

このような共振系の振動抑制と外乱抑圧制御に対して、状態フィードバック制御やＨ∞制御、遅い外乱オブザーバ制御、共振比制御（非特許文献１参照）等の手法が提案されている。

特許第３３８１８８０号公報 結城他「共振比制御による２慣性共振系の振動抑制制御」電学論Ｄ、１１３巻（平成５年）１０号、１１６２頁−１１６９頁

しかしながら、状態フィードバック制御やＨ∞制御は、制御系が複雑であったり、計算量が膨大であることなどから、高速、高機能のＣＰＵが必要となり、実機への適用には問題がある。

これに対して、遅い外乱オブザーバ制御と共振比制御は、比較的簡単な制御系から構成され、実用性が高い。

しかしながら、非特許文献１に記載された共振比制御では、システムを、モータとアームが柔軟な駆動軸により結合された２慣性共振系としてモデル化しているため、１次の共振に対しては抜群の効果があるものの、現実の多慣性系では高次の共振を招いてしまうことがある等、高次の振動に対しての効果は低いという問題点を有していた。

一方、特許文献１には、サーボ系に別体の位相進みフィルタを設けることが記載されているが、構成が複雑であり、細かい計算が必要で計算時間が長くなり、安価な制御装置で実現できなくなる。設計も複雑で、応答が安定なパラメータをみつけにくいという問題点を有していた。

本発明は、前記従来の問題点を解決するべくなされたもので、簡単な構成で、１次の共振モードだけでなく、それより高次の振動モードも含む、全ての共振極の安定化を図ることを課題とする。

本発明は、多慣性共振系の振動抑制制御であって、１次の共振モードに対しては共振比制御を適用し、それより高次の振動モードに対しては位相進み補償を適用するに際して、前記位相進み補償を、共振比制御内の外乱オブザーバで用いるモータ慣性のノミナル値と実際のモータ慣性の値の比率を変化させることで行なうことで、全ての共振極の安定化を図るようにして、前記課題を解決すると共に、別体の位相進み補償手段を不要としたものである。

又、前記位相進み補償の極と零点を、共振比制御の極よりも内側に配置するようにして、位相進み補償と共振比制御を両立させるようにしたものである。

本発明は、又、軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償を行って、全ての共振極の安定化を図るようにしたものである。

本発明は、又、１次の共振モードに対して適用される共振比制御手段と、２次以上の高次の振動モードに対して適用される位相進み補償手段と、を備え、前記共振比制御手段の外乱オブザーバで用いるモータ慣性のノミナル値を実際のモータ慣性の値より大に設定することで前記位相進み補償を行なうようにされていることを特徴とする多慣性共振系の振動抑制制御装置を提供するものである。

又、前記共振比制御手段を、外乱オブザーバと軸ねじれ反力推定オブザーバとから構成したものである。

本発明は、又、軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償が行なわれるようにされていることを特徴とする多慣性共振系の振動抑制制御装置を提供するものである。

本発明においては、共振比制御に加えて、位相進み補償を行なうので、１次の共振モードに対しては２慣性共振系の振動抑制に有効な共振比制御を適用し、それより高次の振動モードに対しては位相進み補償を用いて共振極の安定化を図ることにより、全ての共振極の安定化を図ることができる。

ここで、位相進み補償制御は、例えば共振比制御内の外乱オブザーバで用いるモータ慣性のノミナル値Ｊｍｎを、実際のモータ慣性の値Ｊｍより大（Ｊｍｎ＞Ｊｍ）に設定することで、別体の位相進み補償手段を付加することなく、位相進み補償制御を実現することができる。

又、外乱オブザーバに基づいた制御とした場合には、振動抑制効果を持ちながらロバスト性の確保も可能となる。

更に、状態フィードバック制御やＨ∞制御に比べて制御系が簡単で、演算量も少ないため、高価なＣＰＵ等を使用する必要がない。又、設計や調整も容易である。

又、軸ねじれ反力が無視できる剛性の高い負荷に対しては、外乱オブザーバのみを用いて位相進み補償を行うことができる。

以下図面を参照して、本発明の実施形態を詳細に説明する。

本発明の第１実施形態に係る制御装置の全体のブロック図を図４に示す（図では負荷を２慣性共振系として示しているが、多慣性共振系の場合も同様である）。

本制御装置では、図５に示すような外乱オブザーバ１０と、図７に示すような軸ねじれ反力推定オブザーバ２０を用いて共振比制御を行ない、多慣性共振系の振動抑制制御を行なう。

モータ側に外乱オブザーバ１０を適用することにより、モータに作用する各種外乱の影響を相殺、除去することができ、図６に示すロバストな加速度制御系を構築することができる。

即ち、モータに作用する外乱トルクＴdismは次式のように表わすことができる。

Ｔdism＝（Ｊｍ−Ｊｍｎ）（ｄ²θｍ／ｄｔ²）＋（Ｋtn−Ｋt）Ｉa^ref
＋Ｔfric＋Ｄｍ（ｄθｍ／ｄｔ）＋Ｔreac …（１）

ここで、Ｉa^refは電流参照値を表わし、式の右辺の第１項は慣性変動トルク、第２項はトルクリップル、第３項はクーロン摩擦トルク、第４項は粘性摩擦トルク、第５項は軸ねじれ反力を表わす。

負荷に作用する外乱トルクＴdisaは、軸ねじれ反力Ｔreacに含まれることによりモータへ作用する。電流参照値Ｉａ^refとモータ速度が検出可能であるとき、（１）式で定義さ
れる外乱トルクＴdismは、図５に示される外乱オブザーバ１０により、１次のローパスフィルタを通して、次式のように推定される。図５において、Ｉcmpは外乱トルクを補償し
てロバスト性を確保するための補償電流である。

この推定外乱トルクＴdism^＊をフィードバックすることにより、外乱に対してロバストな制御系を構築することが可能となる。

この外乱オブザーバ１０に基づくロバスト制御系は、図６に示すような加速度制御系となる。外乱オブザーバゲインＧdisを大きく設定することにより外乱トルクＴdismの影響
が除去されることが分かる。これにより、モータは軸ねじれ反力Ｔreacを除去し、負荷側の影響を受けないロバストな制御系となる。

外乱オブザーバ１０をモータ側に適用することにより、唯一の負荷側の情報である軸ねじれ反力Ｔreacを相殺、除去してしまうため、負荷側の振動を誘発することとなる。

そこで、外乱オブザーバ１０とほぼ同構造を持つ軸ねじれ反力推定オブザーバ２０を用いて軸ねじれ反力Ｔreacの推定を行なう。

（１）式の外乱トルクＴdismにおいて、モータ慣性のノミナル値Ｊｍｎを、加速度試験により同定した値とすることで、モータ慣性の変動トルクの影響を除去することが可能である。又、クーロン摩擦トルクＴfric、粘性摩擦トルクＤｍ（ｄθｍ／ｄｔ）を等速度試験により同定し、差し引くことにより、軸ねじれ反力Ｔreacは、次式のように推定される。

Ｔreac^＊＝Ｔdism^＊−Ｔfric−Ｄｍ（ｄθｍ／ｄｔ） …（３）

軸ねじれ反力推定オブザーバ２０のブロック線図を図７に示す。Ｇreacは、軸ねじれ反力推定オブザーバ２０に含まれる１次のローパスフィルタのカットオフ周波数である。

モータ側に外乱オブザーバ１０を適用することにより加速度制御系を構成した制御対象に、軸ねじれ反力Ｔreacをフィードバックした系を図８に示す。Ｋrは軸ねじれ反力Ｔreacのフィードバックゲインであり、任意に設定することができる。

この系における加速度参照値（ｄ²θｍ／ｄｔ²）^refからモータ位置θｍまでの伝達関
数とモータ位置θｍから負荷位置θａまでの伝達関数はそれぞれ以下のようになる。

又、モータ共振周波数ωｍ、及び、負荷共振周波数ωａを、以下のように定義する。

ここで、共振比Ｋを、次式に定義する。

負荷共振周波数ωａは、モータ側には零点として作用する逆共振周波数となる。ωａは、任意パラメータを含まず制御対象により決定される。又、モータ側の状態フィードバックに対して不可制御である。

一方、ωｍはモータ側の共振周波数であり、軸ねじれ反力フィードバックゲインＫrに
より任意に設定できる。

（６）、（７）式を用いることによって、図８は図９のブロック線図へ等価変換される。同図より、モータ側フィードフォワードによる零点操作がなく、モータ側で極零相殺がない限り、負荷側共振極ωａはモータ側逆共振零点と相殺するのが分かる。

共振比制御は軸ねじれ反力Ｔreacをフィードバックするものであり、軸ねじれ反力フィードバックゲインＫrにより共振比Ｋを任意に設定することができる。

共振比Ｋを制御することは、仮想的モータ慣性を制御することに相当し、共振比Ｋが大きいとき、即ちフィードバックゲインＫrが大きい場合には、負荷慣性に対しモータ慣性
が小さくなり、負荷側の影響を受け易くなる。又、逆も同様である。

共振比Ｋを
Ｋ＝√５ …（１０）
に設定することにより、如何なる２慣性共振系に対しても、振動抑制、即応性とも優れるゲイン設定が可能となる。

各ゲインは以下のとおりである。

Ｋr＝４／Ｊa …（１１）
Ｋp＝ωａ² …（１２）
Ｋv＝４ωａ …（１３）

ここで、モータＭに印加される外乱が、パラメータ変動による外乱トルクＴdismのみで構成されていたとすると、次式のように表わされる。

モータの加速度参照値（ｄ²θｍ／ｄｔ²）^refから加速度応答値ｄ²θｍ／ｄｔ²までの
伝達関数を、パラメータ変動を考慮に入れて求めると、次式のようになる。

ここで、トルク定数の変動は十分小さいとして、外乱オブザーバ１０で用いるノミナル値Ｊｍｎ、Ｋｔｎを、以下のように設定する。

Ｊｍｎ＝αＪｍ …（１６）
Ｋｔｎ＝Ｋｔ …（１７）

従来は、Ｊｍ＝Ｊｍｎ、即ち、α＝１となるように制御されている。

（１６）、（１７）式を（１５）式に代入すると、以下の式が得られる。

この（１８）式をブロック線図で表わすと図１０のようになる。

図１０より、加速度参照値（ｄ²θｍ／ｄｔ²）^refに対しては、（ｓ＋Ｇdis）／｛（１／α）ｓ＋Ｇdis｝の位相補償３０を加え、外乱Ｔdismに対しては、ハイパスフィルタのカットオフ周波数をα倍することと等価であると言える。

ここで、
α＜１の場合 位相遅れ補償、外乱オブザーバゲイン低下
α＞１の場合 位相進み補償、外乱オブザーバゲイン増加

即ち、外乱オブザーバ１０で用いるモータ慣性のノミナル値Ｊｍｎと実際のモータ慣性の値Ｊｍの比αを変えることにより、加速度参照値の位相補償と外乱オブザーバのカットオフ周波数を変化させる効果がある。

続いて、多慣性共振系において、外乱オブザーバ１０で用いるモータ慣性のノミナル値Ｊｍｎを変化させた場合の振動抑制効果を根軌跡を用いて示す。

図１１に、多慣性共振系の極（×印）と零点（○印）を複素平面上に図示する。Ｒｅは実軸、Ｉｍは虚軸である。多慣性共振系の極と零点は虚軸Ｉｍ上に交互に並ぶことが分かる。

以下の説明では、簡単のため２慣性共振系を負荷として位相補償を行なった場合について、解析を行なう。２慣性共振系に位相補償を行なった際のブロック線図を図１２に示す。ここで、θcmdは位置指令値（任意に設定できる）、Ｃｐは比例制御のゲインである。

図１２の伝達関数は以下のようになる。

位相補償器３０の値により、特性が変わる。即ち、位相補償器３０は、０＜α＜１の場合、位相遅れ補償となる。このときの極と零点をＰlag、Ｚlagとすると、以下のように表わされる。

Ｐlag＝［０，０，ｊωｍ，−ｊωｍ，−αＧdis］ …（２３）
Ｚlag＝［ｊωａ，−ｊωａ，Ｇdis］ …（２４）

これを図示すると図１３のようになる。×が極、○が零点である。

ここで、位相遅れ補償器３０の極−αＧdisと振動極ｓ＝ｊωｍのなす角をθｐ、反共
振の零点ｓ＝ｊωａのなす角をΦｐとする。又、位相遅れ補償器３０の零点−Ｇdisと振
動極ｓ＝ｉωｍのなす角をθｚ、反共振の零点ｓ＝ｊωａのなす角をΦｚとする。

このとき、それぞれの極の出発角θｉ^ｄ（ｉ＝１〜５）と零点の到着角θｉ^ａ（ｉ＝１〜３）は、以下のように計算される。

θ１^ｄ＝−π
θ２^ｄ＝−π
θ３^ｄ＝θｚ−θｐ＋（π／２）
θ４^ｄ＝−｛θｚ−θｐ＋（π／２）｝
θ１^ａ＝−Φｚ＋Φｐ−（π／２）
θ２^ａ＝−｛−Φｚ＋Φｐ−（π／２）｝

図１４に、３慣性共振系に位相遅れ補償を行なった際の根軌跡を示す。比例制御ゲインＣｐを変化させると、系が必ず不安定の方向に動くことが確認できる。

一方、位相補償器３０は、α＞１の場合、位相進み補償となる。このときの極と零点をＰlead、Ｚleadとすると、以下のように表わされる。

Ｐlead＝［０，０，ｊωｍ，−ｊωｍ，−Ｇdis］ …（２５）
Ｚlead＝［ｊωａ，−ｊωａ，Ｇdis］ …（２６）

これを図示すると図１５のようになる。

ここで、位相進み補償器３０の極−αＧdisと振動極ｓ＝ｊωｍのなす角をθｐ、反共
振の零点ｓ＝ｉωａのなす角をΦｐとする。又、位相進み補償器３０の零点−Ｇdisと共
振極ｓ＝ｉωｍのなす角をθｚ、反共振の零点ｓ＝ｉωａのなす角をΦｚとする。

このとき、それぞれの極の出発角θｉ^ｄ（ｉ＝１〜５）と零点の到着角θｉ^ａ（ｉ＝１〜３）は、以下のように計算される。

θ１^ｄ＝−π
θ２^ｄ＝−π
θ３^ｄ＝θｚ−θｐ＋（π／２）
θ４^ｄ＝−｛θｚ−θｐ＋（π／２）｝
θ１^ａ＝−Φｚ＋Φｐ−（π／２）
θ２^ａ＝−｛−Φｚ＋Φｐ−（π／２）｝

図１６に、３慣性共振系に、本発明による位相進み補償を行なった際の根軌跡を示す。比例制御ゲインＣｐを変化させると、系が必ず安定の方向に動くことが確認できる。

これらは、高次の共振系についても、同様の結果が得られる。即ち、位相進み補償を行なうことで、多慣性共振系の振動抑制制御が可能となることが分かる。

本実施形態では、これらの位相進み補償を、共振比制御内の外乱オブザーバ１０で用いるモータ慣性のノミナル値Ｊｍｎと実際のモータ慣性の値Ｊｍの比率αを変化させることで、特に新たに位相進み補償器３０を追加することなく実現している。

この位相進み補償と共振比制御を両立させるために、位相進み補償の極と零点を、共振比制御の極よりも内側に配置する必要がある。即ち、制御系全体の極配置を図１７のように設定することで、１次の振動モードについては、共振比制御により積極的に抑制を行ない、それより高次の振動モードについては、元々の影響が小さいために、位相進み補償により安定性を確保することが可能となる。

なお、第１実施形態では、モータと負荷とが柔軟な駆動軸により結合され、軸ねじれが問題となるような剛性の低い制御系を対象としていたが、負荷と軸の剛性が高く軸ねじれ補償が必要ないような場合には、図１８に示す第２実施形態のように、軸ねじれ反力推定オブザーバを省略して、外乱オブザーバ１０のみで位相進み補償制御を構成することが可能である。

この第２実施形態においても、位相進み補償の効果により、第１実施形態と同様に全ての共振極に対して安定化を図ることができる。

なお、前記実施形態においては、速度演算部にＰ（比例）制御を用いているが、速度演算の制御の種類はこれに限定されず、ＰＩ（比例積分）制御、ＰＤ（比例微分）制御、ＰＩＤ（比例積分微分）制御等を用いてもよい。また、軸ねじれ反力推定オブザーバを用いる代わりに、リニアエンコーダ等を使用して、負荷側の位置を測定する方法でも可能である。適用対象も、マニピュレータやＸＹロボットに限定されない。

本発明の適用対象の一例を示す構成図 同じくモデル図 同じくブロック図 本発明の第１実施形態の全体ブロック図 第１実施形態で用いられている外乱オブザーバの構成を示すブロック図 前記外乱オブザーバにより構成される加速度制御系のブロック図 第１実施形態で用いられている軸ねじれ反力推定オブザーバの構成を示すブロック図 同じく軸ねじれ反力フィードバックを示すブロック図 図８の等価ブロック線図 第１実施形態で用いられている外乱オブザーバのパラメータ変動による位相補償を示すブロック図 同じく多慣性共振系の極と零点を示す図 同じく２慣性共振系に位相補償を行なった際のブロック線図 ０＜α＜１の比較例における位相遅れ補償時の極と零点の配置を示す図 同じく３慣性共振系に位相遅れ補償を行なった際の根軌跡を示す図 α＞１とした本発明における位相進み補償時の極と零点の配置を示す図 同じく３慣性共振系に本発明による位相進み補償を行なった際の根軌跡を示す図 第１実施形態における極配置を示す図 本発明の第２実施形態の全体ブロック図

符号の説明

Ｍ…モータ
Ａ…負荷
１０…外乱オブザーバ
２０…軸ねじれ反力フィードバック
３０…位相補償器

Claims (6)

1. １次の共振モードに対しては共振比制御を適用し、それより高次の振動モードに対しては位相進み補償を適用するに際して、
   前記位相進み補償を、共振比制御内の外乱オブザーバで用いるモータ慣性のノミナル値と実際のモータ慣性の値の比率を変化させることで行なうことを特徴とする多慣性共振系の振動抑制制御方法。
2. 前記位相進み補償の極と零点を、共振比制御の極よりも内側に配置することを特徴とする請求項１に記載の多慣性共振系の振動抑制制御方法。
3. 軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償を行うことを特徴とする多慣性共振系の振動抑制制御方法。
4. １次の共振モードに対して適用される共振比制御手段と、
   ２次以上の高次の振動モードに対して適用される位相進み補償手段と、
   を備え、
   前記共振比制御手段の外乱オブザーバで用いるモータ慣性のノミナル値を実際のモータ慣性の値より大に設定することで前記位相進み補償を行なうようにされていることを特徴とする多慣性共振系の振動抑制制御装置。
5. 前記共振比制御手段が、外乱オブザーバと軸ねじれ反力推定オブザーバとから構成されることを特徴とする請求項４に記載の多慣性共振系の振動抑制制御装置。
6. 軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償が行なわれるようにされていることを特徴とする多慣性共振系の振動抑制制御装置。

Images