JP2006195566A

JP2006195566A - サーボ制御装置とその制御方法

Info

Publication number: JP2006195566A
Application number: JP2005004279A
Authority: JP
Inventors: Atsushi Hagiwara; 萩原　　淳
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 2005-01-11
Filing date: 2005-01-11
Publication date: 2006-07-27

Abstract

【課題】少ない計算量で、外乱抑圧特性を向上させるだけでなく、完全に位置指令θｒとアーム位置θａを一致させることができるサーボ制御装置を提供する。
【解決手段】制御対象の制御量ｘｄが指令ｒｅｆに一致するようにフィードバック制御器と、フィードフォワード制御器と、オブザーバとを備えたサーボ制御装置において、フィードバック制御器は、オブザーバによる状態量推定値ｓｈにゲインＧを乗じたものを第２の操作量ｔｆｂ２にフィードバックする構成であり、フィードフォワード制御器は、操作量ｔｒｅｆから前記制御量ｘｄまでの伝達関数Ｐ１の逆関数Ｐ１^−１および、制御対象の状態量を検出する検出器の出力ｘｆｂから制御量ｘｄまでの伝達関数Ｐ２の逆関数Ｐ２^−１を含むようにした。
【選択図】図１

Description

本発明は、指令追従性と外乱抑圧特性の両方を少ない演算で同時に向上させることを目的とするサーボ制御装置に関する。

従来のサーボ制御装置は、オブザーバの推定値を用いたフィードバック制御部と指令を微分した信号を作成するフィードフォワード制御部を別々に設計し、両者を併用している。（例えば、特許文献１参照）。

図５においてロボット１０２の第１アーム駆動部２１３は、第１アーム２０３を駆動する駆動電動機及び当該駆動電動機の出力軸に接続された減速機構とからなっている。この第１アーム駆動部２１３は、後述するアンプ部１０５を介して基台２０５に固定され、回動軸に回転力を伝達することで、第１アーム２０３の全体を回動可能にしている。なお、上記第１アーム駆動部２１３やアンプ部１０５はカバー２０６により覆われている。

ロボット１０２の第２アーム駆動部２０７は、例えばスチールベルトからなる駆動ベルト２００を介して第２アーム１０３を駆動するサーボ電動機１０４と、その電動機出力軸に接続された減速機構２１５とからなっている。第２アーム駆動部２０７は、第１アーム駆動部２１３と同様に基台２０５に対して回動方向に固定されている。また支持部２０４は、第２アーム駆動部２０７のサーボ電動機１０４自体が回転しないように、上記カバー２０６を介して上記基台２０５に固定するためのものである。上記サーボ電動機１０４の駆動力は、上記減速機構２１５で減速され、さらに回動軸２０８を介してベルト駆動プーリ２０２に伝達され、このプーリ２０２を回動させるようになっている。なお、これら第１アーム駆動部２１３と第２アーム駆動部２０７は、第１アーム２０３の一方の端部に対応して配置されている。

駆動ベルト２００は、上記ベルト駆動プーリ２０２と第２軸駆動プーリ２０１との間に懸架され、上記第２アーム駆動部２０７にて発生した駆動力を、上記第２軸駆動プーリ２０１に伝達するようになっている。なお、これらベルト駆動プーリ２０２と駆動ベルト２００と第２軸駆動プーリ２０１とは、第１アーム２０３内部に配置されている。

第２軸駆動プーリ２０１は、上記第１アーム２０３の他方の端部に対応して配置され、回動軸２１４を回転中心として回動可能になっている。

第２アーム３の中央部は、上記第２軸駆動プーリ２０１に固着されている。したがって、当該第２軸駆動プーリ２０１が上記駆動ベルト２００を介して上記第２アーム駆動部２０７にて回転されると、上記第２アーム１０３は当該第２軸駆動プーリの回転に伴って駆動されることになる。また、第２アーム１０３の一方の端部には、作用軸２１１を介して、例えば上記基板等に部品を装填するための動作部２１０が配設されている。上記作用軸２１１は、いわゆるツール搭載軸であり、上下方向（Ｚ軸方向）の移動と軸回りの方向（Ｒ軸方向）の回転とを行うことができるものである。したがって、当該作用軸２１１の先端に配設された動作部２１０は、上下，回転方向に自由に駆動されることになり、これにより基板に対して部品を自在に装填可能となる。

上述したように、ロボット１０２は、第１アーム２０３及び第２アーム１０３を有し、第２アーム３が駆動ベルト２００を介して駆動される２慣性系の構成となっている。

また、上記ロボット１０２には、上述した構成の他に、上記第２アーム１０３を駆動するサーボ電動機１０４に対して所望のトルクを発生させる上記アンプ部１０５を備え、さらに上記サーボ電動機１０４の角度を検出する例えばロータリーエンコーダ等からなる角度検出器１０６をも備えている。本発明の実施の形態にかかるロボット１０２のアンプ部１０５には、ロボット制御装置１０１が算出した指令トルクτを示す信号が、コード２１２を介して伝達供給されるようになっており、アンプ部１０５は上記指令トルクτの値に基づいて上記サーボ電動機１０４を駆動する際の上記所望のトルクを発生する。

ここで、上記ロボット１０２は、下記の式（１）に示すように、指令トルクτ、サーボ電動機１０４の角度θｍ、サーボ電動機１０４の角速度θｍ'、第２アーム１０３の角度θａ、第２アーム１０３の角速度θａ'、サーボ電動機１０４の電動機軸に加わる外乱トルクｄを、それぞれ状態変数とした状態方程式で表されるものである。なお、式（１）におけるＪｍはサーボ電動機１０４のイナーシャ、Ｄｍはサーボ電動機１０４の粘性摩擦、Ｊａは第２アーム１０３のイナーシャ、Ｄａは第２アーム１０３の粘性摩擦、Ｋはバネ定数、Ｎはギヤ比である。

すなわち、ロボット１０２では、上記式（１）により表されたダイナミックスに基づいて、サーボ電動機１０４が第２アーム１０３を駆動する。図６において、ねじれ角・外乱推定オブザーバ１０７と、フィードフォワード部１０８と、制御部１０９とを備えており、端子１２０には例えば全体をコントロールするシステムコントローラ等から上記サーボ電動機の所望の目標角度θｒを示す信号が入力される。

上記ねじれ角・外乱推定オブザーバ１０７は、図６に示すように、制御対象であるロボットについて、最小次元オブザーバを構成することにより、ロボットの第２アーム１０３の角度θａ、ロボット２の第２アーム１０３の角速度θａ'、サーボ電動機１０４の角速度θｍ'及び外乱トルクｄを推定する。

すなわち、ねじれ角・外乱推定オブザーバ１０７は、指令トルクτと、角度検出器１０６から検出されたサーボ電動機１０４の実際の角度θｍとが入力され、式（２）、式（３）に示すように、ロボット２の第２アーム１０３の推定角度θａ、第２アーム１０３の推定角速度θａ'、及びサーボ電動機１０４の推定角速度θｍ'を算出し、さらにロボット１０２に外乱として加わる値として推定外乱トルクｄを算出する。これら第２アーム１０３の推定角度θａ、第２アーム１０３の推定角速度θａ'、及びサーボ電動機１０４の推定角速度θｍ'、及び推定外乱トルクｄの各値は、制御部１０９に送られる。

ここで、式（２）、式（３）におけるｚ１、ｚ２、ｚ３、ｚ４は最小次元オブザーバの状態変数、ｋ１、ｋ２、ｋ３、ｋ４は推定速度を決めるパラメータである。

次に、フィードフォワード部１０８は、図６に示すように、上記端子１２０に供給された上記サーボ電動機１０４の所望の目標角度θｒの値が入力され、このサーボ電動機１０４の目標角度θｒを時間微分してサーボ電動機１０４の目標角速度θｒ'を算出して出力する。ここで、サーボ電動機１０４の目標角速度θｒ'は、サーボ電動機１０４の角速度θｍ'と上記サーボ電動機１０４の所望の目標角度θｒとの差分によって求めるようにしても良い。またこのサーボ電動機１０４の目標角速度θｒ'は、サーボ電動機１０４の目標角度θｒが求められるのと同時に求めるようにしても良い。なお、フィードフォワード部１０８において、上記サーボ電動機１０４の目標角速度θｒ'を上記サーボ電動機１０４の所望の目標角度θｒとの差分によって求めるようにする場合には、当該フィードフォワード部１０８に対して上記ねじれ角・外乱推定オブザーバ１０７からサーボ電動機１０４の角速度θｍ'を供給する。上記フィードフォワード部１０８から出力された上記サーボ電動機１０４の目標角速度θｒ'の値は、制御部１０９の後段に設けられている演算部１２１に送られることになる。

制御部１０９には、図６に示すように、端子１２０からのサーボ電動機１０４の目標角度θｒの値と、ねじれ角・外乱推定オブザーバ１０７からの第２アーム１０３の推定角度θａ、第２アーム１０３の推定角速度θａ'、サーボ電動機１０４の推定角速度θｍ'、推定外乱トルクｄの各値と、上記角度検出器１０６にて検出された上記サーボ電動機１０４の角度θｍの値とが入力される。

この制御部１０９及びその後段の演算部１２１では、上記供給された各値を用いて、上記サーボ電動機１０４の角度θｍと第２アーム１０３の推定角度θａとの差分、サーボ電動機１０４の推定角速度θｍ'と第２アーム１０３の推定角速度θａ'との差分、サーボ電動機１０４の目標角速度θｒ'とサーボ電動機１０４の推定角速度θｍ'との差分、サーボ電動機１０４の目標角度θｒとサーボ電動機１０４の角度θｍとの差分をそれぞれ求める。

そして、上記制御部１０９及びその後段の演算部１２１では、下記の式（４）により、上記のように求めたサーボ電動機１０４の角度θｍと第２アーム１０３の推定角度θａとの差分、サーボ電動機１０４の推定角速度θｍ’と第２アーム１０３の推定角速度θａ'との差分、サーボ電動機１０４の目標角速度θｒ'とサーボ電動機１０４の推定角速度θｍ'との差分、サーボ電動機１０４の目標角度θｒとサーボ電動機１０４の角度θｍとの差分、サーボ電動機１０４の目標角速度θｒ'と、前記推定外乱トルクｄとから、指令トルクτを算出して出力する。

なお、上記式（４）におけるｆ１、ｆ２、ｆ３、ｆ４は制御の特性を決める制御パラメータである。
上述したようにして求められた指令トルクτは、制御対象であるロボット１０２に供給される。

ここで、図６のロボット１０２の部分は、より詳細には図７のように表すことができる。すなわち、この図７は、ロボット１０２の各部の動作をブロック線図として表すものであり、図６と対応する構成要素には同一の指示符号を付している。

図７及び図６において、上記ロボット制御装置１０１からの指令トルクτの値は、端子１３１に伝達される。この端子１３１に伝達された指令トルクτの値は、演算部１３３に加算値として入力する。また、この演算部１３３には、端子１３０から前記外乱トルクｄが減算値として伝達される。すなわち、当該演算部１３３では、上記指令トルクτに対する外乱トルクｄの影響量を差し引くことを行っている。この演算部１３３の出力値は、演算部１３４に加算値として入力する。

演算部１３４には、後述する演算部１２４からの出力が減算値として伝達され、当該演算部１３４では上記演算部１３３の出力から演算部１２４の出力値を減算した値を求める。この演算部１３４の出力は、演算部１２３に入力する。なお、上記演算部１３３及び１３４が図６の演算部１２２に対応している。
上記演算部１２３は、演算部１３５及び１３６からなり、前記サーボ電動機４に対応している。上記演算部１２３では、演算部１３５にて前記サーボ電動機１０４のイナーシャＪｍとサーボ電動機１０４の粘性摩擦Ｄｍより、１／（ＪｍＳ＋Ｄｍ）で表される演算を行い、さらに演算部１３６にて上記演算部１３５の出力値を積分する演算を行い、これら演算｛１／（ＪｍＳ＋Ｄｍ）Ｓ｝により得られた値を出力する。すなわち、当該演算部１２３において、上記演算部１３５の出力値は上記サーボ電動機１０４の角速度θｍ'に対応し、上記演算部１３６の出力値は上記サーボ電動機１０４の速度θｍに対応する。なお、上記式中のＳはラプラス変換演算子である。

上述したようなことから、演算部１２２から演算部１２３までのブロックは、制御対象であるロボット１０２における指令トルクτに基づいた実際のサーボ電動機４の駆動と、このサーボ電動機１０４の駆動に応じた角度検出器１０６における角度検出動作とに対応している。上記角度検出器１０６にて検出されたサーボ電動機１０４の角度θｍは、端子１３９を介してロボット制御装置１０１にフィードバックされることになる。

上記演算部１２３の出力値は、演算部１２５に伝達され、この演算部１２５にて１／Ｎ倍になされる。すなわち、上記Ｎは前記ギヤ比であり、当該演算部１２５は、上記サーボ電動機１０４の電動機出力軸に接続された前記減速機構２１５と対応している。この演算部１２５の出力値は、第２アーム１０３の角度θａに相当し、この値が演算部１２６に加算値として伝達される。

上記演算部１２６から演算部１２８までの構成は、ロボット１０２の第２アーム１０３に対応している。上記演算部１２６には、後述する演算部１２８から伝達されたロボット１０２の第２アーム１０３の実際の角度θａが、減算値として供給されている。すなわちこの演算部１２６では、演算部１２８から伝送される第２アーム１０３の実際の角度θａを、上記演算部１２５から伝達される角度θａまで変化させる際の、角度変化量を求めている。

上記演算部１２６の出力値は、演算部１２７に伝達され、ここで前記バネ定数であるＫが掛けられた後、さらに演算部１２４にて１／Ｎ倍されて、前記演算部１３４に減算値として伝達される。すなわち、上記演算部１２７及び１２４では、上記サーボ電動機１０４の角度の値をサーボ電動機１０４のトルクの値に変換している。これにより、前記演算部１３４では、前記演算部１３３で求められたトルクの値から現在のサーボ電動機４のトルクの値が差し引かれた値が求められる。

また、上記演算部１２７の出力値は、演算部１２８にも伝達される。当該演算部１２８は、演算部１３７及び１３８からなる。当該演算部１２８では、演算部１３７にて前記第２アーム１０３のイナーシャＪａと第２アーム１０３の粘性摩擦Ｄａより、１／（ＪａＳ＋Ｄａ）で表される演算を行い、さらに演算部１３８にて上記演算部１３７の出力値を積分する演算を行い、これら演算｛１／（ＪａＳ＋Ｄａ）Ｓ｝により得られた値を出力する。すなわち、当該演算部１２８において、上記演算部１３７の出力値は上記第２アーム１０３の角速度θａ'に対応し、上記演算部１３８の出力値は上記第２アーム１０３の角度θａに対応する。

上記演算部１２８にて求められた上記第２アーム１０３の実際の角度θａは、端子１２９から外部へ作用する力として取り出されることになる。

上述したように、従来例のロボット制御装置１０１においては、ねじれ角と外乱とを同時に推定するねじれ角・外乱推定手段を備えたことにより、剛性の低い２慣性のロボット１０２に対して外乱に強い制御を行うので、ロボット１０２側の第２アーム１０３の振動の抑制が図られるとともに、当該ロボット１０２側の第２アーム１０３の位置決めの高速化が図られる。したがって、このロボット制御装置１０１によれば、ロボット１０２による電子部品の組み立て等の作業効率の向上が図られるというものである。

このように、従来のサーボ制御装置は、フィードフォワードとオブザーバを別々に設計し、それらを組み合わせて使用している。
特開平９−２１２２０３号公報（第９−１３頁、図１，２，３）

従来のサーボ制御装置は、たとえば説明を簡単にするために減速比Ｎ＝１とし、粘性係数ＤｍおよびＤａを０とした時、位置指令θｒからアーム位置θａまでの伝達関数は式（５）のようになる。

式（５）から明らかなように、パラメータｆ１、ｆ２、ｆ３、ｆ４を操作して極配置することは可能であるため振動抑制や外乱特性の改善をすることはできるが、分母が４次、分子が１次の伝達関数で表されるため、完全に位置指令θｒとアーム位置θａを一致させる（伝達関数を１とする）ことは不可能である。このように、従来の制御装置では、外乱抑圧特性は向上させることができるが、指令追従性には限界があり、かつ、計算量が多くなるという問題があった。
本発明は、このような問題点に鑑みてなされたものであり、少ない計算量で、外乱抑圧特性を向上させるだけでなく、完全に位置指令θｒとアーム位置θａを一致させることができるサーボ制御装置を提供することを目的とする。

請求項１記載の本発明は、制御対象の制御量ｘｄが指令ｒｅｆに一致するようにフィードバック制御器と、フィードフォワード制御器と、オブザーバとを備えたサーボ制御装置において、前記フィードバック制御器は、前記オブザーバによる状態量推定値ｓｈにゲインＧを乗じたものを第２の操作量ｔｆｂ２にフィードバックする構成であり、前記フィードフォワード制御器は、操作量ｔｒｅｆから前記制御量ｘｄまでの伝達関数Ｐ１の逆関数Ｐ１^−１および、制御対象の状態量を検出する検出器の出力ｘｆｂから制御量ｘｄまでの伝達関数Ｐ２の逆関数Ｐ２^−１を含むことを特徴とするものである。
請求項２記載の本発明は、請求項１記載のサーボ制御装置において、前記オブザーバは、制御対象に作用する外乱を推定するものであることを特徴とするものである。
請求項３記載の本発明は、請求項１記載のサーボ制御装置において、前記オブザーバは、制御対象が２慣性系の場合に、ねじれ角とねじれ角速度と負荷速度のうちの少なくとも一つを推定することを特徴とするものである。
請求項４記載の本発明は、請求項１記載のサーボ制御装置において、前記オブザーバは、制御対象が機台振動系の場合に、機台の変位と速度のうちの少なくとも一つを推定することを特徴とするものである。
請求項５記載の本発明は、請求項１記載のサーボ制御装置において、前記オブザーバの推定値をフィードバックする際に、オブザーバ推定値ｓｈをフィルタ処理した後に、ゲインＧを乗じたものをフィードバックすることを特徴とするものである。
請求項６記載の本発明は、請求項１記載のサーボ制御装置において、前記フィードフォワード制御器は、分母が２次以上のフィルタと伝達関数Ｐ１の逆伝達関数Ｐ１^−１および伝達関数Ｐ２の逆伝達関数Ｐ２^−１から構成されていることを特徴とするものである。
請求項７記載の本発明は、制御対象の制御量ｘｄが指令ｒｅｆに一致するようにフィードバック制御器と、フィードフォワード制御器と、オブザーバとを備えたサーボ制御方法において、前記位置指令ｒｅｆをフィルタを通してｒｅｆ１を求めるステップと、トルク指令ｔｒｅｆがら負荷位置までの逆伝達関数Ｐ１^−１を通して前記ｒｅｆ１からｔｆｆをもとめるステップと、電動機位置ｘｆｂから負荷位置ｘｄまでの伝達関数Ｐ２の逆関数Ｐ２^−１を通して前記ｒｅｆ１からｘｆｆをもとめるステップと、前記ｘｆｆと前記ｘｆｂの差からＰＩＤ制御器を通して第１のトルク指令ｔｆｂ１を求めるステップと、前記ｔｆｆと前記ｔｆｂ１を加算して第２のトルク指令ｔｆｂ２と求めるステップと、前記オブサーバで最終の操作量ｔｒｅｆと位置検出器出力ｘｆｂから状態推定値ｓｈをもとめるステップと、前記状態推定値ｓｈをフィルタを通してゲインＧを乗じｔｏｂｓをもとめるステップと、前記ｔｆｂ２から前記ｔｏｂｓを減算してｔｒｅｆをもとめるステップと、前記ｔｒｅｆで前記制御対象を駆動することを特徴とするものである。

請求項１に記載の発明によると、オブザーバ推定値のフィードバック制御の設計を行った後に、全体のシステムに対して、その逆特性を有するようにフィードフォワード制御部の設計を行うため、特別な処理を追加することなく、少ない計算量で、指令に対しても外乱に対しても良好な性能が得られるサーボ制御装置を実現することができる。
請求項２に記載の発明によると、オブザーバで外乱を推定値フィードバックすることで、制御対象に作用する外乱や実際のイナーシャとノミナルイナーシャの誤差を除去することができるため、フィードフォワード制御部では、外乱やモデル化誤差の影響を無視して、ノミナルなモデルの逆特性をもつ制御器を設計すれば、良好な性能を実現することができる。
請求項３記載の発明によると、制御対象が２慣性系の時に、オブザーバで、ねじれ角やねじれ角速度や負荷速度を推定し、ゲインを乗じてフィードバックすることで、システムの減衰を大きくすることができ、且つ、フィードフォワード制御器では減衰を上げた後の制御対象の逆特性をもつ制御器を設計すれば、指令に対しても外乱に対しても良好な性能が得られるサーボ制御装置を実現することができる。
請求項４記載の発明によると、制御対象が機台振動系の時に、オブザーバで、機台の変位と速度を推定し、ゲインを乗じてフィードバックすることで、システムの減衰を大きくすることができ、且つ、フィードフォワード制御器では減衰を上げた後の制御対象の逆特性をもつ制御器を設計すれば、指令に対しても外乱に対しても良好な性能が得られるサーボ制御装置を実現することができる。
請求項５記載の発明によると、オブザーバ推定値ｓｈをフィルタ処理することで、フィルタの設定により、抽出したい信号のみにゲインを乗じてフィードバックすることが可能であり、複数の外乱が作用するような場合も、良好な性能が得られるサーボ制御装置を実現することができる。
請求項６記載の発明によると、分母が２次以上のフィルタを用いることで、逆特性を用いても滑らかなフィードフォワード信号を作成することができ、結果として良好な性能が得られるサーボ制御装置を実現することができる。

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

図１は、本発明のサーボ制御装置のブロック図である。図において、１はフィードバック制御器、２はフィードフォワード制御器、３はオブザーバ、４は制御対象で電動機５と負荷６で構成される。７は電動機位置検出器、８はＰＩＤ補償器、９は第１の逆伝達関数Ｐ１^−１、１０は第２の逆伝達関数Ｐ２^−１、１１はゲインＧである。フィードフォワード制御器２は位置指令ｒｅｆから位置フィードフォワード信号ｘｆｆおよびトルクフィードフォワード指令ｔｆｆを生成する。フィードバック制御器１は、位置フィードフォワード信号ｘｆｆと電動機位置ｘｆｂの差からＰＩＤ制御を行い、第１のトルク指令ｔｆｂ１を生成する。さらに、第１のトルク指令ｔｆｂ１とトルクフィードフォワード指令ｔｆｆを加算し、第２のトルク指令ｔｆｂ２を生成する。オブザーバ３は、トルク指令ｔｒｅｆと電動機位置ｘｆｂから制御対象の状態量や外乱の推定値ｓｈを生成する。ゲインＧ１１は状態推定量ｓｈにゲインＧを乗算して、推定トルクｔｏｂｓを生成する。減算器は第２のトルク指令から推定トルクを減算してトルク指令ｔｒｅｆを生成する。トルク指令ｔｒｅｆは図示していないが電流指令に変換され、電流制御器で電動機にトルクを発生させる。

フィードフォワード制御器内部の処理として、第１の逆伝達関数９は、トルク指令値ｔｆｂ２から制御量ｘｄまでの伝達関数Ｐ１の逆数であるＰ１^−１の計算を行う。第２の逆伝達関数１０は、電動機の位置ｘｆｂから制御量ｘｄまでの伝達関数Ｐ２の逆数であるＰ２^−１の計算を行う。

本発明が従来例と異なる部分は、フィードフォワード制御器２内部の処理である。

以下、各処理に関して詳細に説明する。
図３は本実施例の詳細ブロック図である。
本実施例では、図６の従来例の構成と比較し以下のようにする。
４の制御対象は図６の１０２と同じ構成、３のオブザーバは図６の１０７と同じ構成、８のＰＩＤ制御器は図６の１０９と同じ構成である。また、減速比Ｎ＝１とし、粘性係数ＤｍおよびＤａを０、外乱ｄ＝０とし、１１のゲインＧは、以下のＫｐ，Ｋｄ，Ｋｓ，Ｋｓｄからなり、従来例のｆ１、ｆ２、ｆ３、ｆ４に相当するものとする。
各記号は式（６）のように対応しているとする。
ｘｄ＝ θａ
ｘｆｂ＝ θｍ
ｘｄ' ＝θａ'
ｘｆｂ' ＝ θｍ'
ｔｒｅｆ＝τ
Ｋｓ＝ｆ１
Ｋｓｄ＝ｆ２
Ｋｄ＝ｆ３
Ｋｐ＝ｆ４
ｘｓ＝ｘｆｂ−ｘｄ
ｘｓ’ ＝ｘｆｂ−ｘｄ'
（６）

指令θｒの替わりにフィードフォワード制御器２から出力されるｘｆｆを用い、フィードフォワード信号−ｆ５・θｒ'の替わりにフィードフォワード制御器２から出力されるｔｆｆを用いるとすると、トルク指令値ｔｒｅｆ（従来例の指令トルクτと同等）は式（７）のようになる。

ここで、ｔｆｂ２からｘｄの伝達関数Ｐ１および、ｘｆｂからｘｄの伝達関数Ｐ２は、それぞれ式（８）および式（９）のようになる。

そこで、式（８）および式（９）からｘｆｆとｔｆｆを求めると式（１０）と式（１１）のようになる。
ｘｆｆ＝Ｐ１^−１・ｒｅｆ（１０）
ｔｆｆ＝Ｐ２^−１・ｒｅｆ（１１）

ここで、式（１０）や式（１１）のＰ１^−１やＰ２^−１の中にでてくる制御対象の物理パラメータＪｍ，Ｊａ，Ｋの値はわからないので、それらの変数のノミナル値をそれぞれ、Ｊｍｎ，Ｊａｍ，Ｋｎとし、式（７）のｘｆｆとｔｆｆに代入した後、位置指令ｒｅｆからアーム位置ｘｄまでの伝達関数を導出すると式（１２）が得られる。

式（１２）からわかるように、分母は、従来の制御装置の伝達特性である式（５）と全く同等である。このことから、従来の制御装置同様に、パラメータｆ１、ｆ２、ｆ３、ｆ４に相当する、Ｋｐ，Ｋｄ，Ｋｓ，Ｋｓｄを操作して極配置することは可能であるため振動抑制や外乱特性の改善をすることはできる。
それに加えて、制御対象の実際のＪｍ，Ｊａ，Ｋの値とノミナル値Ｊｍｎ，Ｊａｍ，Ｋｎが一致した場合には、分母と分子が完全に同じになり、位置指令ｒｅｆから制御対象の位置ｘｄまでの伝達特性が１となる。このように、指令追従性に関しても理想的な制御装置を構成できるのである。

ここでは、従来例に合わせて、制御対象を２慣性系とし、オブザーバ３は従来例で負荷位置および負荷速度を推定するような構成の説明をしたが、本発明は制御対象によらない。例えば、制御対象が剛体の場合は、
Ｐ１＝１／（Ｊｍ・ｓ^２）（１３）
Ｐ２＝１（１４）
とするだけである。
また、本発明にて制御対象が機台振動系の場合は、図４に示すようになる。この場合、オブザーバ３で推定する状態量は、機台の位置ｘｂと速度ｘｂ’とする。この場合は、Ｐ１およびＰ２は式（１５）、式（１６）のようにすればよいだけである。

Ｐ２＝１（１６）

この場合、Ｐ１^−１が虚軸上に極を持つことになり振動が発生する。このような場合は、
式（１５）の分母をＰ１^−１とし、式（１５）の分子をＰ２^−１とすればよい。
このように、外乱抑圧特性を向上させるためにＦＢ系を設計し、その後、制御系も含めた部分を含めて新しい制御対象とし、その逆特性を有するようにフィードフォワード制御部を設計する構成になっているので、制御対象によらず、指令追従性と外乱抑圧特性両方の性能向上を簡単な計算で実現することができるのである。

図２は第２実施例の構成を示す図である。図１の構成と基本的には同じであるが、２のフィードフォワード制御器内部の処理が、９の逆伝達関数１と１０の逆伝達関数２に加えて、１４のフィルタを有している点と、３のオブザーバの推定値ｓｈに直接ゲインＧを乗じてｔｏｂｓを作成するのではなく、推定値ｓｈを１３のフィルタ処理した後、ゲインＧを乗じてｔｏｂｓを作成する点が異なる。

このように、１３のフィルタの設定により、抽出したい信号のみにゲインを乗じてフィードバックすることが可能となり、複数の外乱が作用するような場合も、良好な性能が得られるサーボ制御装置を実現することができる。また、１４のフィルタとして、分母が２次以上のフィルタを用いることで、逆特性に微分要素がある場合も、分母の次数を上げることで滑らかなフィードフォワード信号を作成することができ、結果として良好な性能が得られる。

次に本発明の制御方法について説明する。図８は本発明の制御方法を示すフローチャートで制御周期ごとに実行される。ステップＳＴ１で、位置指令ｒｅｆをフィルタを通してｒｅｆ１を求め、ステップＳＴ２で、トルク指令ｔｒｅｆがら負荷位置までの逆伝達関数Ｐ１^−１を通して前記ｒｅｆ１からｔｆｆをもとめ、ステップＳＴ３で、電動機位置ｘｆｂから負荷位置ｘｄまでの伝達関数Ｐ２の逆関数Ｐ２^−１を通して前記ｒｅｆ１からｘｆｆをもとめ、ステップＳＴ４で、ｘｆｆとｘｆｂの差からＰＩＤ制御器を通して第１のトルク指令ｔｆｂ１を求め、ステップＳＴ５で、ｔｆｆとｔｆｂ１を加算して第２のトルク指令ｔｆｂ２と求め、ステップＳＴ６で、オブサーバでトルク指令値ｔｒｅｆと位置検出器出力ｘｆｂから状態推定値ｓｈをもとめ、ステップＳＴ７で、状態推定値ｓｈをフィルタを通してゲインＧを乗じｔｏｂｓをもとめ、ステップＳＴ８で、ｔｆｂ２から前記ｔｏｂｓを減算してｔｒｅｆをもとめ、ステップＳＴ９で、ｔｒｅｆで制御対象を駆動する。

本発明は、指令追従性と外乱抑圧特性両方の性能向上を簡単な計算で実現できるので、工作機やロボットだけではなく一般産業機械などにも幅広く適用できる。

本発明の第１実施例を示すサーボ制御装置のブロック図本発明の第２実施例のサーボ制御装置の構成を説明するブロック図制御対象が２慣性系の場合の本発明の第１実施例を示す詳細ブロック図制御対象が機台振動系の場合の本発明の第１実施例を示す詳細ブロック図従来のシステム全体を説明する構成図従来のサーボ制御装置のブロック図従来の制御対象を示すブロック図本発明の方法を示すフローチャート

符号の説明

１フィードバック制御器
２フィードフォワード制御器
３オブザーバ
４制御対象
５電動機
６負荷
７検出器
８ＰＩＤ制御器
９逆伝達関数１
１０逆伝達関数２
１１ゲインＧ
１３フィルタ
１４フィルタ
１０１ロボット制御装置
１０２ロボット
１０３第２アーム
１０４サーボ電動機
１０５アンプ部
１０６角度検出器
１０７外乱オブザーバ
１０８フィードフォワード部
１０９制御部
１２２〜１２９演算部
１３０端子
１３１端子
１３３〜１３６演算部
２００駆動ベルト
２０１第２軸駆動プーリ
２０２ベルト駆動プーリ
２０３第１アーム
２０４支持部
２０５基台
２０６カバー
２０７第２アーム駆動部
２０８回転軸
２１０動作部
２１１作用軸
２１２コード
２１３第１アーム駆動部
２１４回転軸
２１５減速機構

Claims

制御対象の制御量ｘｄが指令ｒｅｆに一致するようにフィードバック制御器と、フィードフォワード制御器と、オブザーバとを備えたサーボ制御装置において、
前記フィードバック制御器は、前記オブザーバによる状態量推定値ｓｈにゲインＧを乗じたものを第２の操作量ｔｆｂ２にフィードバックする構成であり、
前記フィードフォワード制御器は、操作量ｔｒｅｆから前記制御量ｘｄまでの伝達関数Ｐ１の逆関数Ｐ１^−１および、制御対象の状態量を検出する検出器の出力ｘｆｂから制御量ｘｄまでの伝達関数Ｐ２の逆関数Ｐ２^−１を含むことを特徴とするサーボ制御装置。
前記オブザーバは、制御対象に作用する外乱を推定することを特徴とする請求項１記載のサーボ制御装置。
前記オブザーバは、制御対象が２慣性系の場合に、ねじれ角とねじれ角速度と負荷速度のうちの少なくとも一つを推定することを特徴とする請求項１記載のサーボ制御装置。
前記オブザーバは、制御対象が機台振動系の場合に、機台の変位と速度のうちの少なくとも一つを推定することを特徴とする請求項１記載のサーボ制御装置。
前記オブザーバの推定値をフィードバックする際に、オブザーバ推定値ｓｈをフィルタ処理した後に、ゲインＧを乗じたものをフィードバックすることを特徴とする請求項１乃至４記載のサーボ制御装置。
前記フィードフォワード制御器は、分母が２次以上のフィルタと伝達関数Ｐ１の逆伝達関数Ｐ１^−１および伝達関数Ｐ２の逆伝達関数Ｐ２^−１から構成されていることを特徴とする請求項１乃至４記載のサーボ制御装置。
制御対象の制御量ｘｄが指令ｒｅｆに一致するようにフィードバック制御器と、フィードフォワード制御器と、オブザーバとを備えたサーボ制御方法において、
前記指令ｒｅｆをフィルタを通してｒｅｆ１を求めるステップと、
操作量ｔｒｅｆから制御量ｘｄまでの伝達関数Ｐ１の逆関数Ｐ１^−１を通して前記ｒｅｆ１からｔｆｆを求めるステップと、
制御対象の状態量を検出する検出器の出力ｘｆｂから制御量ｘｄまでの伝達関数Ｐ２の逆関数Ｐ２^−１を通して前記ｒｅｆ１からｘｆｆを求めるステップと、
前記ｘｆｆと前記ｘｆｂの差からＰＩＤ制御器を通して第１の操作量ｔｆｂ１を求めるステップと、
前記ｔｆｆと前記ｔｆｂ１を加算して第２の操作量ｔｆｂ２と求めるステップと、
前記オブサーバで最終の操作量ｔｒｅｆと前記検出器の出力ｘｆｂから状態推定値ｓｈを求めるステップと、
前記状態推定値ｓｈをフィルタを通してゲインＧを乗じｔｏｂｓを求めるステップと、
前記ｔｆｂ２から前記ｔｏｂｓを減算してｔｒｅｆを求めるステップと、
前記ｔｒｅｆで前記制御対象を駆動することを特徴とするサーボ制御方法。