(57)【要約】【課題】多慣性共振系の全ての共振モードを安定化する。【解決手段】１次の共振モードに対しては、２慣性共振系の振動抑制に有効な共振比制御を適用し、それより高次の振動モードに対しては、位相進み補償を用いて、全ての共振極を安定化する。一方、軸ねじれ反力が無視できる剛性の高い負荷に対しては、外乱オブザーバのみを用いて位相進み補償を行うことで、全ての共振極を安定化する。【選択図】図４
2007043884.tif 000002
【特許請求の範囲】
【請求項１】
１次の共振モードに対しては共振比制御を適用し、それより高次の振動モードに対しては位相進み補償を適用することを特徴とする多慣性共振系の振動抑制制御方法。
【請求項２】
前記位相進み補償を、共振比制御内の外乱オブザーバで用いるモータ慣性のノミナル値と実際のモータ慣性の値の比率を変化させることで行なうことを特徴とする請求項１に記載の多慣性共振系の振動抑制制御方法。
【請求項３】
前記位相進み補償の極と零点を、共振比制御の極よりも内側に配置することを特徴とする請求項１又は２に記載の多慣性共振系の振動抑制制御方法。
【請求項４】
軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償を行うことを特徴とする多慣性共振系の振動抑制制御方法。
【請求項５】
１次の共振モードに対して適用される共振比制御手段と、
２次以上の高次の振動モードに対して適用される位相進み補償手段と、
を備えたことを特徴とする多慣性共振系の振動抑制制御装置。
【請求項６】
前記共振比制御手段が、外乱オブザーバと軸ねじれ反力推定オブザーバとから構成されることを特徴とする請求項５に記載の多慣性共振系の振動抑制制御装置。
【請求項７】
軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償が行なわれるようにされていることを特徴とする多慣性共振系の振動抑制制御装置。
【請求項８】
前記外乱オブザーバで用いるモータ慣性のノミナル値を実際のモータ慣性の値より大に設定することで前記位相進み補償を行なうようにされていることを特徴とする請求項５又は６に記載の多慣性共振系の振動抑制制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、多慣性共振系の振動抑制制御方法及び装置に係り、特に、フレキシブルマニピュレータやＸＹロボット等に用いられる、モータ制御装置の振動抑制制御に関する。
【背景技術】
【０００２】
一般に、フレキシブルマニピュレータや、ボールねじやタイミングベルト等を用いたＸＹロボット等のモータ駆動系においては、図１に例示する如く、モータＭと負荷Ａが、剛性よりも軽量化を優先して低剛性の弾性軸（図ではプーリＰ間に掛け渡されたベルトＢ）で結合されていると共振系となり、軸ねじれ振動等が発生し問題となることがある。
【０００３】
実際の共振系は、多数の振動モードや固有周波数が存在するため、図２に例示する如く、多慣性共振系としてモデル化される。図２において、ＪｍはモータＭの慣性、Ｋｆ１、Ｋｆ２・・・は、ばね定数、Ｊａ１、Ｊａ２・・・Ｊａｎは負荷Ａの慣性である。
【０００４】
この多慣性共振系は、図３に示すようなブロック図で表わされる。図３において、θｍはモータＭの回転角度（モータ位置）、θａは負荷Ａの回転角度（負荷位置）、Ｔはトルク、ｓはラプラス演算子、添字ｍはモータ、添字ａは負荷、添字disは外乱、添字reacは軸ねじれ反力を表わす。
【０００５】
このような共振系の振動抑制と外乱抑圧制御に対して、状態フィードバック制御やＨ∞制御、遅い外乱オブザーバ制御、共振比制御（非特許文献１参照）等の手法が提案されている。
【０００６】
【特許文献１】特許第３３８１８８０号公報
【非特許文献１】結城他「共振比制御による２慣性共振系の振動抑制制御」電学論Ｄ、１１３巻（平成５年）１０号、１１６２頁−１１６９頁
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、状態フィードバック制御やＨ∞制御は、制御系が複雑であったり、計算量が膨大であることなどから、高速、高機能のＣＰＵが必要となり、実機への適用には問題がある。
【０００８】
これに対して、遅い外乱オブザーバ制御と共振比制御は、比較的簡単な制御系から構成され、実用性が高い。
【０００９】
しかしながら、非特許文献１に記載された非共振制御では、システムを、モータとアームが柔軟な駆動軸により結合された２慣性共振系としてモデル化しているため、１次の共振に対しては抜群の効果があるものの、現実の多慣性系では高次の共振を招いてしまうことがある等、高次の振動に対しての効果は低いという問題点を有していた。
【００１０】
一方、特許文献１には、サーボ系に別体の位相進みフィルタを設けることが記載されているが、構成が複雑であり、細かい計算が必要で計算時間が長くなり、安価な制御装置で実現できなくなる。設計も複雑で、応答が安定なパラメータをみつけにくいという問題点を有していた。
【００１１】
本発明は、前記従来の問題点を解決するべくなされたもので、簡単な構成で、１次の共振モードだけでなく、それより高次の振動モードも含む、全ての共振極の安定化を図ることを課題とする。
【課題を解決するための手段】
【００１２】
本発明は、多慣性共振系の振動抑制制御に際して、１次の共振モードに対しては共振比制御を適用し、それより高次の振動モードに対しては位相進み補償を適用することで、全ての共振極の安定化を図るようにして、前記課題を解決したものである。
【００１３】
又、前記位相進み補償を、共振比制御内の外乱オブザーバで用いるモータ慣性のノミナル値と実際のモータ慣性の値の比率を変化させることで行なうようにして、別体の位相進み補償手段を不要としたものである。
【００１４】
又、前記位相進み補償の極と零点を、共振比制御の極よりも内側に配置するようにして、位相進み補償と共振比制御を両立させるようにしたものである。
【００１５】
本発明は、又、軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償を行って、全ての共振極の安定化を図るようにしたものである。
【００１６】
本発明は、又、１次の共振モードに対して適用される共振比制御手段と、２次以上の高次の振動モードに対して適用される位相進み補償手段と、を備えたことを特徴とする多慣性共振系の振動抑制制御装置を提供するものである。
【００１７】
又、前記共振比制御手段を、外乱オブザーバと軸ねじれ反力推定オブザーバとから構成したものである。
【００１８】
本発明は、又、軸ねじれ反力が無視できる剛性の高い負荷に対して、外乱オブザーバのみを用いて位相進み補償が行なわれるようにされていることを特徴とする多慣性共振系の振動抑制制御装置を提供するものである。
【００１９】
又、前記外乱オブザーバで用いるモータ慣性のノミナル値を実際のモータ慣性の値より大に設定することで前記位相進み補償を行なうようにして、別体の位相進み補償手段を不要としたものである。
【発明の効果】
【００２０】
本発明においては、共振比制御に加えて、位相進み補償を行なうので、１次の共振モードに対しては２慣性共振系の振動抑制に有効な共振比制御を適用し、それより高次の振動モードに対しては位相進み補償を用いて共振極の安定化を図ることにより、全ての共振極の安定化を図ることができる。
【００２１】
ここで、位相進み補償制御は、例えば共振比制御内の外乱オブザーバで用いるモータ慣性のノミナル値Ｊｍｎを、実際のモータ慣性の値Ｊｍより大（Ｊｍｎ＞Ｊｍ）に設定することで、別体の位相進み補償手段を付加することなく、位相進み補償制御を実現することができる。
【００２２】
又、外乱オブザーバに基づいた制御とした場合には、振動抑制効果を持ちながらロバスト性の確保も可能となる。
【００２３】
更に、状態フィードバック制御やＨ∞制御に比べて制御系が簡単で、演算量も少ないため、高価なＣＰＵ等を使用する必要がない。又、設計や調整も容易である。
【００２４】
又、軸ねじれ反力が無視できる剛性の高い負荷に対しては、外乱オブザーバのみを用いて位相進み補償を行うことができる。
【発明を実施するための最良の形態】
【００２５】
以下図面を参照して、本発明の実施形態を詳細に説明する。
【００２６】
本発明の第１実施形態に係る制御装置の全体のブロック図を図４に示す（図では負荷を２慣性共振系として示しているが、多慣性共振系の場合も同様である）。
【００２７】
本制御装置では、図５に示すような外乱オブザーバ１０と、図７に示すような軸ねじれ反力推定オブザーバ２０を用いて共振比制御を行ない、多慣性共振系の振動抑制制御を行なう。
【００２８】
モータ側に外乱オブザーバ１０を適用することにより、モータに作用する各種外乱の影響を相殺、除去することができ、図６に示すロバストな加速度制御系を構築することができる。
【００２９】
即ち、モータに作用する外乱トルクＴdismは次式のように表わすことができる。
【００３０】
Ｔdism＝（Ｊｍ−Ｊｍｎ）（ｄ2θｍ／ｄｔ2）＋（Ｋtn−Ｋt）Ｉaref
＋Ｔfric＋Ｄｍ（ｄθｍ／ｄｔ）＋Ｔreac              …（１）
【００３１】
ここで、Ｉarefは電流参照値を表わし、式の右辺の第１項は慣性変動トルク、第２項はトルクリップル、第３項はクーロン摩擦トルク、第４項は粘性摩擦トルク、第５項は軸ねじれ反力を表わす。
【００３２】
負荷に作用する外乱トルクＴdisaは、軸ねじれ反力Ｔreacに含まれることによりモータへ作用する。電流参照値Ｉａrefとモータ速度が検出可能であるとき、（１）式で定義さ
れる外乱トルクＴdismは、図５に示される外乱オブザーバ１０により、１次のローパスフィルタを通して、次式のように推定される。図５において、Ｉcmpは外乱トルクを補償し
てロバスト性を確保するための補償電流である。
【００３３】
【数１】
2007043884.tif 000003
【００３４】
この推定外乱トルクＴdism＊をフィードバックすることにより、外乱に対してロバストな制御系を構築することが可能となる。
【００３５】
この外乱オブザーバ１０に基づくロバスト制御系は、図６に示すような加速度制御系となる。外乱オブザーバゲインＧdisを大きく設定することにより外乱トルクＴdismの影響
が除去されることが分かる。これにより、モータは軸ねじれ反力Ｔreacを除去し、負荷側の影響を受けないロバストな制御系となる。
【００３６】
外乱オブザーバ１０をモータ側に適用することにより、唯一の負荷側の情報である軸ねじれ反力Ｔreacを相殺、除去してしまうため、負荷側の振動を誘発することとなる。
【００３７】
そこで、外乱オブザーバ１０とほぼ同構造を持つ軸ねじれ反力推定オブザーバ２０を用いて軸ねじれ反力Ｔreacの推定を行なう。
【００３８】
（１）式の外乱トルクＴdisにおいて、モータ慣性のノミナル値Ｊｍｎを、加速度試験
により同定した値とすることで、モータ慣性の変動トルクの影響を除去することが可能である。又、クーロン摩擦トルクＴfric、粘性摩擦トルクＤｍ（ｄ2θｍ／ｄｔ2）を等速度試験により同定し、差し引くことにより、軸ねじれ反力Ｔreacは、次式のように推定される。
【００３９】
Ｔreac＊＝Ｔdism＊−Ｔfric−Ｄｍ（ｄθｍ／ｄｔ）    …（３）
【００４０】
軸ねじれ反力推定オブザーバ２０のブロック線図を図７に示す。Ｇreacは、軸ねじれ反力推定オブザーバ２０に含まれる１次のローパスフィルタのカットオフ周波数である。
【００４１】
モータ側に外乱オブザーバ１０を適用することにより加速度制御系を構成した制御対象に、軸ねじれ反力Ｔreacをフィードバックした系を図８に示す。Ｋrは軸ねじれ反力Ｔreacのフィードバックゲインであり、任意に設定することができる。
【００４２】
この系における加速度参照値（ｄ2θｍ／ｄｔ2）refからモータ位置θｍまでの伝達関
数とモータ位置θｍから負荷位置θａまでの伝達関数はそれぞれ以下のようになる。
【００４３】
【数２】
2007043884.tif 000004
【００４４】
又、モータ共振周波数ωｍ、及び、負荷共振周波数ωａを、以下のように定義する。
【００４５】
【数３】
2007043884.tif 000005
【００４６】
ここで、共振比Ｋを、次式に定義する。
【００４７】
2007043884.tif 000006
【００４８】
負荷共振周波数ωａは、モータ側には零点として作用する逆共振周波数となる。ωａは、任意パラメータを含まず制御対象により決定される。又、モータ側の状態フィードバックに対して不可制御である。
【００４９】
一方、ωｍはモータ側の共振周波数であり、軸ねじれ反力フィードバックゲインＫrに
より任意に設定できる。
【００５０】
（６）、（７）式を用いることによって、図８は図９のブロック線図へ等価変換される。同図より、モータ側フィードフォワードによる零点操作がなく、モータ側で極零相殺がない限り、負荷側共振極ωａはモータ側逆共振零点と相殺するのが分かる。
【００５１】
共振比制御は軸ねじれ反力Ｔreacをフィードバックするものであり、軸ねじれ反力フィードバックゲインＫrにより共振比Ｋを任意に設定することができる。
【００５２】
共振比Ｋを制御することは、仮想的モータ慣性を制御することに相当し、共振比Ｋが大きいとき、即ちフィードバックゲインＫrが大きい場合には、負荷慣性に対しモータ慣性
が小さくなり、負荷側の影響を受け易くなる。又、逆も同様である。
【００５３】
共振比Ｋを
Ｋ＝√５                  …（１０）
に設定することにより、如何なる２慣性共振系に対しても、振動抑制、即応性とも優れるゲイン設定が可能となる。
【００５４】
各ゲインは以下のとおりである。
【００５５】
Ｋr＝４／Ｊa              …（１１）
Ｋp＝ωａ2                …（１２）
Ｋv＝４ωａ               …（１３）
【００５６】
ここで、モータＭに印加される外乱が、パラメータ変動による外乱トルクＴdismのみで構成されていたとすると、次式のように表わされる。
【００５７】
2007043884.tif 000007
【００５８】
モータの加速度参照値（ｄ2θｍ／ｄｔ2）refから加速度応答値ｄ2θｍ／ｄｔ2までの
伝達関数を、パラメータ変動を考慮に入れて求めると、次式のようになる。
【００５９】
【数４】
2007043884.tif 000008
【００６０】
ここで、トルク定数の変動は十分小さいとして、外乱オブザーバ１０で用いるノミナル値Ｊｍｎ、Ｋｔｎを、以下のように設定する。
【００６１】
Ｊｍｎ＝αＪｍ          …（１６）
Ｋｔｎ＝Ｋｔ            …（１７）
【００６２】
従来は、Ｊｍ＝Ｊｍｎ、即ち、α＝１となるように制御されている。
【００６３】
（１６）、（１７）式を（１５）式に代入すると、以下の式が得られる。
【００６４】
【数５】
2007043884.tif 000009
【００６５】
この（１８）式をブロック線図で表わすと図１０のようになる。
【００６６】
図１０より、加速度参照値（ｄ2θｍ／ｄｔ2）refに対しては、（ｓ＋Ｇdis）／｛（１／α）ｓ＋Ｇdis｝の位相補償３０を加え、外乱Ｔdismに対しては、ハイパスフィルタのカットオフ周波数をα倍することと等価であると言える。
【００６７】
ここで、
α＜１の場合    位相遅れ補償、外乱オブザーバゲイン低下
α＞１の場合    位相進み補償、外乱オブザーバゲイン増加
【００６８】
即ち、外乱オブザーバ１０で用いるモータ慣性のノミナル値Ｊｍｎと実際のモータ慣性の値Ｊｍの比αを変えることにより、加速度参照値の位相補償と外乱オブザーバのカットオフ周波数を変化させる効果がある。
【００６９】
続いて、多慣性共振系において、外乱オブザーバ１０で用いるモータ慣性のノミナル値Ｊｍｎを変化させた場合の振動抑制効果を根軌跡を用いて示す。
【００７０】
図１１に、多慣性共振系の極（×印）と零点（○印）を複素平面上に図示する。Ｒｅは実軸、Ｉｍは虚軸である。多慣性共振系の極と零点は虚軸Ｉｍ上に交互に並ぶことが分かる。
【００７１】
以下の説明では、簡単のため２慣性共振系を負荷として位相補償を行なった場合について、解析を行なう。２慣性共振系に位相補償を行なった際のブロック線図を図１２に示す。ここで、θcmdは位置指令値（任意に設定できる）、Ｃｐは比例制御のゲインである。
【００７２】
図１２の伝達関数は以下のようになる。
【００７３】
【数６】
2007043884.tif 000010
2007043884.tif 000011
【００７４】
位相補償器３０の値により、特性が変わる。即ち、位相補償器３０は、０＜α＜１の場合、位相遅れ補償となる。このときの極と零点をＰlag、Ｚlagとすると、以下のように表わされる。
【００７５】
Ｐlag＝［０，０，ｊωｍ，−ｊωｍ，−αＧdis］      …（２３）
Ｚlag＝［ｊωａ，−ｊωａ，Ｇdis］                  …（２４）
【００７６】
これを図示すると図１３のようになる。×が極、○が零点である。
【００７７】
ここで、位相遅れ補償器３０の極−αＧdisと振動極ｓ＝ｊωｍのなす角をθｐ、反共
振の零点ｓ＝ｊωａのなす角をΦｐとする。又、位相遅れ補償器３０の零点−Ｇdisと振
動極ｓ＝ｉωｍのなす角をθｚ、反共振の零点ｓ＝ｊωａのなす角をΦｚとする。
【００７８】
このとき、それぞれの極の出発角θｉｄ（ｉ＝１〜５）と零点の到着角θｉａ（ｉ＝１〜３）は、以下のように計算される。
【００７９】
θ１ｄ＝−π
θ２ｄ＝−π
θ３ｄ＝θｚ−θｐ＋（π／２）
θ４ｄ＝−｛θｚ−θｐ＋（π／２）｝
θ１ａ＝−Φｚ＋Φｐ−（π／２）
θ２ａ＝−｛−Φｚ＋Φｐ−（π／２）｝
【００８０】
図１４に、３慣性共振系に位相遅れ補償を行なった際の根軌跡を示す。比例制御ゲインＣｐを変化させると、系が必ず不安定の方向に動くことが確認できる。
【００８１】
一方、位相補償器３０は、α＞１の場合、位相進み補償となる。このときの極と零点をＰlead、Ｚleadとすると、以下のように表わされる。
【００８２】
Ｐlead＝［０，０，ｊωｍ，−ｊωｍ，−Ｇdis］      …（２５）
Ｚlead＝［ｊωａ，−ｊωａ，Ｇdis］                …（２６）
【００８３】
これを図示すると図１５のようになる。
【００８４】
ここで、位相進み補償器３０の極−αＧdisと振動極ｓ＝ｊωｍのなす角をθｐ、反共
振の零点ｓ＝ｉωａのなす角をΦｐとする。又、位相進み補償器３０の零点−Ｇdisと共
振極ｓ＝ｉωｍのなす角をθｚ、反共振の零点ｓ＝ｉωａのなす角をΦｚとする。
【００８５】
このとき、それぞれの極の出発角θｉｄ（ｉ＝１〜５）と零点の到着角θｉａ（ｉ＝１〜３）は、以下のように計算される。
【００８６】
θ１ｄ＝−π
θ２ｄ＝−π
θ３ｄ＝θｚ−θｐ＋（π／２）
θ４ｄ＝−｛θｚ−θｐ＋（π／２）｝
θ１ａ＝−Φｚ＋Φｐ−（π／２）
θ２ａ＝−｛−Φｚ＋Φｐ−（π／２）｝
【００８７】
図１６に、３慣性共振系に、本発明による位相進み補償を行なった際の根軌跡を示す。比例制御ゲインＣｐを変化させると、系が必ず安定の方向に動くことが確認できる。
【００８８】
これらは、高次の共振系についても、同様の結果が得られる。即ち、位相進み補償を行なうことで、多慣性共振系の振動抑制制御が可能となることが分かる。
【００８９】
本実施形態では、これらの位相進み補償を、共振比制御内の外乱オブザーバ１０で用いるモータ慣性のノミナル値Ｊｍｎと実際のモータ慣性の値Ｊｍの比率αを変化させることで、特に新たに位相進み補償器３０を追加することなく実現している。
【００９０】
この位相進み補償と共振比制御を両立させるために、位相進み補償の極と零点を、共振比制御の極よりも内側に配置する必要がある。即ち、制御系全体の極配置を図１７のように設定することで、１次の振動モードについては、共振比制御により積極的に抑制を行ない、それより高次の振動モードについては、元々の影響が小さいために、位相進み補償により安定性を確保することが可能となる。
【００９１】
なお、第１実施形態では、モータと負荷とが柔軟な駆動軸により結合され、軸ねじれが問題となるような剛性の低い制御系を対象としていたが、負荷と軸の剛性が高く軸ねじれ補償が必要ないような場合には、図１８に示す第２実施形態のように、軸ねじれ反力推定オブザーバを省略して、外乱オブザーバ１０のみで位相進み補償制御を構成することが可能である。
【００９２】
この第２実施形態においても、位相進み補償の効果により、第１実施形態と同様に全ての共振極に対して安定化を図ることができる。
【００９３】
なお、前記実施形態においては、速度演算部にＰ（比例）制御を用いているが、速度演算の制御の種類はこれに限定されず、ＰＩ（比例積分）制御、ＰＤ（比例微分）制御、ＰＩＤ（比例積分微分）制御等を用いてもよい。また、軸ねじれ反力推定オブザーバを用いる代わりに、リニアエンコーダ等を使用して、負荷側の位置を測定する方法でも可能である。適用対象も、マニピュレータやＸＹロボットに限定されない。
【図面の簡単な説明】
【００９４】
【図１】本発明の適用対象の一例を示す構成図
【図２】同じくモデル図
【図３】同じくブロック図
【図４】本発明の第１実施形態の全体ブロック図
【図５】第１実施形態で用いられている外乱オブザーバの構成を示すブロック図
【図６】前記外乱オブザーバにより構成される加速度制御系のブロック図
【図７】第１実施形態で用いられている軸ねじれ反力推定オブザーバの構成を示すブロック図
【図８】同じく軸ねじれ反力フィードバックを示すブロック図
【図９】図８の等価ブロック線図
【図１０】第１実施形態で用いられている外乱オブザーバのパラメータ変動による位相補償を示すブロック図
【図１１】同じく多慣性共振系の極と零点を示す図
【図１２】同じく２慣性共振系に位相補償を行なった際のブロック線図
【図１３】０＜α＜１の比較例における位相遅れ補償時の極と零点の配置を示す図
【図１４】同じく３慣性共振系に位相遅れ補償を行なった際の根軌跡を示す図
【図１５】α＞１とした本発明における位相進み補償時の極と零点の配置を示す図
【図１６】同じく３慣性共振系に本発明による位相進み補償を行なった際の根軌跡を示す図
【図１７】第１実施形態における極配置を示す図
【図１８】本発明の第２実施形態の全体ブロック図
【符号の説明】
【００９５】
Ｍ…モータ
Ａ…負荷
１０…外乱オブザーバ
２０…軸ねじれ反力フィードバック
３０…位相補償器
【図１】
2007043884.tif 000012
【図２】
2007043884.tif 000013
【図３】
2007043884.tif 000014
【図４】
2007043884.tif 000015
【図５】
2007043884.tif 000016
【図６】
2007043884.tif 000017
【図７】
2007043884.tif 000018
【図８】
2007043884.tif 000019
【図９】
2007043884.tif 000020
【図１０】
2007043884.tif 000021
【図１１】
2007043884.tif 000022
【図１２】
2007043884.tif 000023
【図１３】
2007043884.tif 000024
【図１４】
2007043884.tif 000025
【図１５】
2007043884.tif 000026
【図１６】
2007043884.tif 000027
【図１７】
2007043884.tif 000028
【図１８】
2007043884.tif 000029