(57)【特許請求の範囲】
【請求項１】
フィードフォワード制御部２にメカのモデルを有し、前記モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部２の位置、速度、トルクのうち少なくとも一つの状態量をフィードバック制御部１の指令とする２自由度制御方法において、前記フィードバック制御部１にオブザーバ処理部１３を有し、前記フィードフォワード制御部２にもオブザーバ処理部２７を有し、前記フィードフォワード制御部２のオブザーバ処理部２７の推定値と前記フィードバック制御部１のオブザーバ処理部１３の推定値の偏差を求め、該偏差にゲイン１４を乗じて前記フィードバック制御部１の速度指令値あるいはトルク指令値に加算することを特徴とするサーボ制御方法。
【請求項２】
前記フィードフォワード制御部２のオブザーバ処理部２７および前記フィードバック制御部１のオブザーバ処理部１３の推定値をフィルタ処理することを特徴とする請求項１記載のサーボ制御方法。
【請求項３】
前記フィードフォワード制御部２のオブザーバ処理部２７の推定値と前記フィードバック制御部１のオブザーバ処理部１３の推定値の偏差を前記フィルタ１５に入力して、前記ゲイン１４を乗じて前記フィードバック制御部１の状態量に加算することを特徴とする請求項１または２記載のサーボ制御方法。
【請求項４】
フィードフォワード制御部２にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部２の位置、速度、トルクのうち少なくとも一つの状態量を前記フィードバック制御部１の指令とする２自由度制御方法において、該フィードバック制御部１にオブザーバ処理部１３を有し、前記フィードフォワード制御部２にもオブザーバ処理部２７を有し、前記フィードフォワード制御部２のオブザーバ処理部２７の推定値と前記フィードバック制御部１のオブザーバ処理部１３の推定値の偏差を求め、該偏差を監視することで、前記メカの状態を診断することを特徴とするサーボ制御方法。
【請求項５】
フィードフォワード制御部２にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部２の位置、速度、トルクのうち少なくとも一つの状態量をフィードバック制御部１の指令とする２自由度制御方法において、
フィードバック制御部１にオブザーバ処理部１３を有し、フィードフォワード制御部２にもオブザーバ処理部２７を有し、前記フィードフォワード制御部２のオブザーバ処理部２７の推定値と前記フィードバック制御部１のオブザーバ処理部１３の推定値の偏差を求め、前記偏差が小さくなるように、今回の前記偏差にゲインを乗じた信号を次回試行時のトルク指令値に加算するということを繰り返し学習制御を行うことを特徴とするサーボ制御方法。
【請求項６】
フィードフォワード制御部２にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部２の位置、速度、トルクのうち少なくとも一つの状態量をフィードバック制御部１の指令とする２自由度制御方法において、
前記フィードバック制御部１にオブザーバ処理部１３を有し、前記フィードフォワード制御部２にもオブザーバ処理部２７を有し、前記フィードフォワード制御部２のオブザーバ処理部２７の推定値と前記フィードバック制御部１のオブザーバ処理部１３の推定値の偏差を求め、前記偏差の大きさを評価基準として、前記偏差が小さくなるようにパラメータを繰り返し修正し、メカのモデルのパラメータを同定することを特徴とするサーボ制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、工作機やロボットアームなどの２自由度制御方法に関し、特にモデル化誤差がある場合の振動抑制や、オブザーバによる速度推定値フィードバック時の振動抑制を行うサーボ制御方法に関するものである。また、オブザーバの推定値を使用して、故障診断や異常検出を行い、繰り返し学習制御を行うとともに、実機モデルの同定を行うサーボ制御方法に関するものである。
【０００２】
【従来の技術】
従来のサーボ制御方法において、指令応答の改善のために、指令応答を司るフィードフォワード制御器と外乱応答を司るフィードバック制御器を独立に設計できる２自由度制御が用いられていた。
その中で特開平８−１６８２８０公報では、「電動機の速度制御装置並びに速度及び位置制御装置」として以下のような方法が記載されている。以下、簡単に図面を用いて説明する。
特開平８−１６８２８０では、先ず、従来例として、図１０に示す構成を挙げている。
図１０において１がフィードバック（以下ＦＢと記述）制御部であり、２がフィードフォワード（以下ＦＦと記述）制御部である。図に示される記号の意味を以下に示す。
Kp：フィードバック制御部の位置ループ比例ゲイン
Kv：フィードバック制御部の速度ループ比例ゲイン
Ki：フィードバック制御部の速度ループ積分ゲイン
Kp2：フィードフォワード制御部の位置ループ比例ゲイン
Kv2：フィードフォワード制御部の速度ループ比例ゲイン
Jm2：フィードフォワード制御部のモデルのモータイナーシャ
Ｓ：ラプラス演算子
θref：位置指令値
θ：モータ位置
ｖ：モータ速度
ｕ：モータ操作量
θff：フィードフォワードモデルモータ位置
vff：フィードフォワードモデルモータ速度
uff：フィードフォワードモデルモータ操作量
ここで、３はモータ操作量どおりにモータを動作させる為の指令を作成するトルク制御回路であり、４はモータ、５は制御対象を表わすものとする。６はモータの位置と速度を検出する位置速度検出器である。この方法によれば指令応答と外乱応答を独立に設計できるという効果がある。
しかしながら、この構成では、制御対象が剛体の時には問題ないが、ＸＹテーブルやロボットアームに使われるボールネジや減速器などのトルク伝達機構の剛性が低い場合、目標値応答を上げる為にフィードフォワード制御ゲインを上げたりすると、応答が振動的になってしまうという課題があると記述されている。これに対して、特開平８−１６８２８０公報では、図９に示す構成を示している。
図９において、各記号の意味は図１０と同様である。その他の記号の意味を以下に示す。
Ks2：ねじれ角ＦＢゲイン
Ksd2：ねじれ角速度ＦＢゲイン
K2：ＦＦモデルバネ定数
JL2：ＦＦモデル負荷イナーシャ
図１０の構成に２４、２５，２６が追加された構成になっている。このようにメカのモデルとして、バネ要素Ｋ２と負荷イナーシャＪＬ２を考慮した上でＦＦ制御器を構成することで、トルク伝達機構の剛性が低い場合も、オーバーシュートや振動が極めて少ない高応答な制御が実現できると記述されている。
【０００３】
【発明が解決しようとする課題】
しかしながら、上記従来の「電動機の速度制御装置並びに速度及び位置制御装置」において、実際の制御対象と、ＦＦモデルに誤差がある場合は、残留振動が発生する。例として、モデルがずれた時のシミュレーション結果を図７に示す。
図８（ａ）は実機とモデルが一致している時、図８（ｂ）は実際の負荷イナーシャがＦＦモデルの負荷イナーシャの１．５倍の時である。
本シミュレーション時の各パラメータ値を以下に示す。
Kp：40（1/S）、Kv：375（1/S）、Ki：20（1/S）
Jm：1.16e-4（kg・m^2）、JL：4.53e-4*1.5（kg・m^2）、K：0.23（N・m/rad）
Jm2：1.16e-4（kg・m^2）、JL2：4.53e-4（kg・m^2）、K2：0.23（N・m/rad）
本来なら図８（ａ）のように応答させたいにもかかわらず、図８（ｂ）に示すように、残留振動が発生している。このように、位置決め時に残留振動が発生すると、残留振動がおさまるまで次の作業に移行できず、大きな問題になる。
したがって、この発明の目的は上記課題を解決することができ、位置決め時に残留振動のないサーボ制御方法を提供することである。
【０００４】
【課題を解決するための手段】
請求項１記載のサーボ制御方法は、フィードフォワード制御部にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部の少なくとも一つの状態量をフィードバック制御部の指令とする２自由度制御方法において、該フィードバック制御部にオブザーバ処理部を有し、前記フィードフォワード制御部にもオブザーバ処理部を有し、フィードフォワード制御部のオブザーバの推定値とフィードバック制御部のオブザーバの推定値の偏差を求め、該偏差にゲインを乗じて前記フィードバック制御部の状態量に加算することを特徴とするものである。
請求項２記載のサーボ制御方法は、前記フィードフォワード制御部のオブザーバおよび前記フィードバック制御部のオブザーバの推定値をフィルタ処理する事を特徴とするものである。
請求項３記載のサーボ制御方法は、前記フィードフォワード制御部のオブザーバの推定値と前記フィードバック制御部のオブザーバの推定値の偏差をフィルタ処理して、ゲインを乗じて前記フィードバック制御部の状態量に加算することを特徴とするものである。
請求項４記載のサーボ制御方法は、フィードフォワード制御部にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部の少なくとも一つの状態量をフィードバック制御部の指令とする２自由度制御方法において、該フィードバック制御部にオブザーバ処理部を有し、前記フィードフォワード制御部にもオブザーバ処理部を有し、前記フィードフォワード制御部のオブザーバの推定値と前記フィードバック制御部のオブザーバの推定値の偏差を求め、該偏差を監視することで、メカの状態を診断することを特徴とするものである。
請求項５記載のサーボ制御方法は、フィードフォワード制御部にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部の少なくとも一つの状態量をフィードバック制御部の指令とする２自由度制御方法において、該フィードバック制御部にオブザーバ処理部を有し、フィードフォワード制御部にもオブザーバ処理部を有し、フィードフォワード制御部のオブザーバの推定値とフィードバック制御部のオブザーバの推定値の偏差を求め、該偏差が小さくなるように繰り返し学習制御を行うことを特徴とするものである。
請求項６記載のサーボ制御方法は、フィードフォワード処理部にメカのモデルを有し、該モデルが所望の動作を行うように制御器を構成し、前記フィードフォワード制御部の少なくとも一つの状態量をフィードバック制御部の指令とする２自由度制御方法において、該フィードバック制御部にオブザーバ処理部を有し、前記フィードフォワード制御部にもオブザーバ処理部を有し、前記フィードフォワード制御部のオブザーバの推定値と前記フィードバック制御部のオブザーバの推定値の偏差を求め、該偏差の大きさを監視しながら、前記メカのモデルのパラメータを同定することを特徴とするものである。
【０００５】
【発明の実施の形態】
この発明の実施の形態について、図に基づいて説明する。
実施例１：図１は本発明の第1の実施の形態の構成を示す図であり、図1において、各記号の意味は図９と図１０で説明したものと同じである。図１において新しく追加された部分は、１３に示すオブザーバ、１４に示すＦＢゲインＧ、２７に示すＦＦ部に構成されたオブザーバの３つである。
２７のオブザーバで推定された状態量と、１３のオブザーバで推定された状態量の差に１４に示すゲインＧを乗じてＦＢ制御器にフィードバックする構成をとる。
次に、１３と２７に示すオブザーバの構成例を以下に示す。式（１）が連続系での実機モデル式で、式（２）が式（１）を離散化しオブザーバの形にしたものである。実際には式（２）を使用し、ｋ番目の値を使用してｋ＋１番目の各変数を逐次計算することになる。
【０００６】
【数１】
0003871030.tif 000002
【０００７】
本方式を用いたシミュレーション結果を図７に示す。
各パラメータの値は図８のシミュレーションで示した値と同じである。
図８（ｂ）に対して残留振動が速やかに減衰している様子が分かる。
以上のように実施例１の方法によれば、ＦＦモデルと実機の間にモデル化誤差があった場合も、振動を抑制することが可能になるという効果がある。
ここでは、オブザーバとして剛体外乱オブザーバを紹介したが、この他の構成のオブザーバでも当然、同様の結果が得られる。
また、本実施例では実機モデルとして２慣性系のモデルを用いているが、実機に合わせて、どのようなモデルを構成しても、本実施例と同様の効果が得られる。
【０００８】
実施例２：ここでは、請求項１記載のサーボ制御方法の二つ目の実施例について図２を用いて説明する。
制御演算遅れが原因でフィードバックゲインが上げられない問題に対して、オブザーバを構成しオブザーバの速度推定値をフィードバックすることで、演算遅れを補償する方法が一般的に知られている。この場合も、図２に示すようにＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部の
【０００９】
【数２】
0003871030.tif 000003
【００１０】
とＦＢ部の
【００１１】
【数３】
0003871030.tif 000004
【００１２】
を使用することにより、振動を誘発せずに演算遅れを改善できるという効果が得られる。
【００１３】
実施例３：ここでは、請求項２および請求項３記載のサーボ制御方法について説明する。構成は図３に示す。図３中１５はフィルタを表わす。フィルタとしては、ローパスフィルタ、ハイパスフィルタ、バンドパスフィルタ、ノッチフィルタ等どれを用いてもよい。
いま、モータにある周期外乱が作用する場合を考える。この場合、振動を抑制する為に、通常、外乱推定オブザーバを使用し、
【００１４】
【数４】
0003871030.tif 000005
【００１５】
を、フィルタ処理後ゲインを乗じてＦＢする方法が知られている。しかしながら、オブザーバを剛体系で構成している場合、バネ要素を有するメカの場合に、周期外乱以外に、自分自身のねじれトルクまでもが、外乱として推定されてしまい（式（４））、これをＦＢすると、加速あるいは減速時にかえって振動が発生したり、軌跡が悪化したりすることがある。この問題を解決する為に、本方式では、ＦＦ部にもオブザーバを有し、ＦＦ部の
【００１６】
【数５】
0003871030.tif 000006
【００１７】
からＦＢ部の
【００１８】
【数６】
0003871030.tif 000007
【００１９】
を差し引くことで、ねじれトルクの推定値分はキャンセルされ、式（５）に示すように、本当にＦＢしたい周期外乱のみが抽出できる。
【００２０】
【数７】
0003871030.tif 000008
【００２１】
以上、実施例３によれば、外乱推定値から真の外乱のみを抽出することが可能になり、それをフィルタ処理した後にＦＢすれば、良好な制御特性を有することが可能になる。
【００２２】
実施例４：ここでは、請求項４記載のサーボ制御方法について図４を用いて説明する。
制御するメカが故障、破損したり、他の機器等と衝突した際は、ＦＢ部のオブザーバの外乱推定値を監視することで検出可能である。本方式ではＦＦ部とＦＢ部両方にオブザーバを構成し、外乱推定値の偏差を監視するので、実施例２で説明したように、ねじれトルクが存在する場合でも、真の外乱のみを抽出することができ、結果として精度良く、診断できる。
図４中７は異常監視部で、式（６）の判定を行うものとする。
【００２３】
【数８】
0003871030.tif 000009
【００２４】
式（６）の条件式を満たした場合は、異常と判断するようにｌｉｍｉｔの大きさを設定しておけばよい。
【００２５】
実施例５：ここでは、請求項５記載のサーボ制御方法について図５を用いて説明する。
周期振動などの場合、繰り返し学習制御が効果的に用いられることがある。
ここでも、図５に示す構成で、ＦＦ部とＦＢ部のオブザーバの外乱推定値の差を求め、それにゲインＧを乗じて次回試行時のトルク指令値に加えて、動作させるといったことを繰り返し行うことで、周期振動をなくすことが可能になる。ここでも、ＦＦ部とＦＢ部両方にオブザーバを構成し、外乱推定値の偏差を用いるので、ねじれトルクが存在する場合でも、真の外乱のみを抽出することができるため、結果として精度良く学習できるという効果がある。
【００２６】
実施例６：ここでは、請求項６記載のサーボ制御方法について図６を用いて説明する。
図６中８はオブザーバの偏差を入力とするパラメータ同定部である。
ここでも、ＦＦ部とＦＢ部にそれぞれオブザーバを構成する。その二つの偏差を評価基準として、その値が、小さくなるように、パラメータを決定もしくは修正する。誤差がある場合は、偏差が大きくなり、モデルが一致した時に偏差は小さくなる。
オブザーバ推定値としては、負荷位置推定値、負荷速度推定値、外乱推定値、ねじれ角推定値、ねじれ角速度推定値など、どの推定値を使用しても良い。また、波形を確認しながら、人がパラメータを調整する際も、推定値どうしを比較することにより、調整がし易くなるという効果がある。
【００２７】
【発明の効果】
請求項１記載のサーボ制御方法によれば、ＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部のオブザーバ推定値と、ＦＢ部のオブザーバの推定値の偏差を求める為、モデルの誤差分のみを抽出でき、その値にゲインを乗じてＦＢすることで、モデル化誤差がある場合も残留振動を抑制することが可能である。
請求項２記載のサーボ制御方法によれば、ＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部のオブザーバ推定値と、ＦＢ部のオブザーバの推定値の偏差を求める為、モデルの誤差分のみを抽出できるとともに、オブザーバ推定値から特別な周波数成分のみを抽出したり、位相を調整できる為、高精度に制御することができる。
請求項３記載のサーボ制御方法によれば、ＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部のオブザーバ推定値と、ＦＢ部のオブザーバの推定値の偏差を求める為、モデルの誤差分のみを抽出できるとともに、オブザーバ推定値から特別な周波数成分のみを抽出したり、位相を調整できる為、高精度に制御することができる。
請求項４記載のサーボ制御方法によれば、ＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部のオブザーバ推定値と、ＦＢ部のオブザーバの推定値の偏差を求める為、モデルの誤差分および真の外乱のみを抽出できるので、精度良く異状診断を行うことができる。
請求項５記載のサーボ制御方法によれば、ＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部のオブザーバ推定値と、ＦＢ部のオブザーバの推定値の偏差を求める為、モデルの誤差分および真の外乱のみを抽出できるため、精度良く学習することができる。
請求項６記載のサーボ制御方法によれば、ＦＦ部とＦＢ部両方にオブザーバを設け、ＦＦ部のオブザーバ推定値と、ＦＢ部のオブザーバの推定値の偏差を求める為、モデルの誤差分のみを抽出できるため、精度良くモデルを同定することができる。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態の構成を示すブロック図
【図２】本発明の第２の実施の形態の構成を示すブロック図
【図３】本発明の第３の実施の形態の構成を示すブロック図
【図４】本発明の第４の実施の形態の構成を示すブロック図
【図５】本発明の第５の実施の形態の構成を示すブロック図
【図６】本発明の第６の実施の形態の構成を示すブロック図
【図７】本発明の方法のシミュレーション結果（モデルが一致していない場合）
【図８】（ａ）従来の方法のシミュレーション結果（モデルが一致している場合）、（ｂ）従来の方法のシミュレーション結果（モデルが一致していない場合）
【図９】従来の方法の構成を示すブロック図
【図１０】従来の方法における従来構成を示すブロック図
【符号の説明】
１  フィードバック制御部
２  フィードフォワード制御部
３  トルク制御回路
４  モータ
５  制御対象
６  位置速度検出器
７  異常監視部
８  パラメータ同定部
１１  位置ループ処理
１２  速度ループ処理
１３  ＦＢ（フィードバック）部オブザーバ処理部
１４  ゲイン
１５  フィルタ
１６  むだ時間
２１  ＦＦ（フィードフォワード）位置ループ処理
２２  ＦＦ速度ループ処理
２３  ＦＦモータモデル
２４  ＦＦモデルバネ定数
２５  ＦＦモデルイナーシャ部
２６  ＦＦねじれ角フィードバック部
２７  ＦＦ部オブザーバ処理部
【図１】
0003871030.tif 000010
【図２】
0003871030.tif 000011
【図３】
0003871030.tif 000012
【図４】
0003871030.tif 000013
【図５】
0003871030.tif 000014
【図６】
0003871030.tif 000015
【図７】
0003871030.tif 000016
【図８】
0003871030.tif 000017
【図９】
0003871030.tif 000018
【図１０】
0003871030.tif 000019