(57)【特許請求の範囲】
【請求項１】
２軸以上の複数の制御軸を有する制御対象に対し、各軸回りの重力モーメントを計算する演算機構と、各軸のバネ定数を計算する演算機構と、前記バネ定数計算値を用いて制御系の状態変数を推定するためのオブザーバ推定行列を計算する演算機構と、前記オブザーバ推定行列計算値の転送手段と、オブザーバ推定行列計算値を用いて状態変数の推定を行うオブザーバ演算機構とを持ち、オブザーバ推定行列計算時の離散化を状態方程式の段階で１次で終了させ、各軸のバネ定数は、あらかじめいくつかの重力モーメント値におけるバネ定数を実験でもとめておき、実験で求めたバネ定数を有理関数で近似し、必要精度に応じて代表点を増減して演算することを特徴とするオブザーバ制御演算装置。
【請求項２】
前記オブザーバ演算機構は位置・速度ループ演算機構内にあって、位置、速度ループのサンプリング時間と独立にサンプリング時間を設定することを特徴とする請求項１記載のオブザーバ制御演算装置。
【請求項３】
コントローラ上位側でロボットの手先目標値から、各軸の関節角指令値を求め、これをコントローラ下位側に転送し、各軸の位置、速度のフィードバック制御を行うディジタル制御装置に、オブザーバ制御演算装置を接続し、オブザーバ制御演算装置の上位側では、ロボットアームの各部に設けられた質点の重さと回転軸からの距離、各軸の関節角をもとに重力モーメント演算機構で重力モーメント値を求め、その重力モーメント値を使ってバネ定数演算機構でバネ定数を求め、前記重力モーメント値と前記バネ定数をもとにオブザーバ推定行列を求め、そのオブザーバ推定行列をオブザーバ制御演算装置の下位側に転送すると共に、オブザーバ制御演算装置の下位側では、上位側から転送されたオブザーバ推定行列とコントローラ下位側からモータの速度検出信号とモータへのトルク指令値を受け取り、位置・速度ループのサンプリング時間とは独立に、ねじれ角及び負荷速度の推定を行い、それぞれのフィードバックゲインと乗算し、位置・速度ループから出力されたトルク指令値から減算することを特徴とする請求項１または請求項２に記載のオブザーバ制御演算装置。
【請求項４】
前記制御対象は、ロボットであることを特徴とする請求項１ないし請求項３までのいずれかに記載のオブザーバ制御演算装置。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、ロボットのような低剛性な機械系を制御するディジタル制御装置にオブザーバ演算機構を組み込む場合において、推定精度をできるだけ落とさずに演算時間を短くでき、制振効果の安定性を高くできるオブザーバ制御演算に関する。
【０００２】
【従来の技術】
ロボットの機構部のような剛性の低い機械系の制御を行う場合、振動問題の制御的解決手段の一つとしてオブザーバ制御技術が利用されている。ロボットのような非線形な制御対象を線形近似したモデルにオブザーバを適用する際に、重力モーメント値の変動による推定精度の悪化の問題があるが、計算した重力モーメントとバネ定数を用いて推定行列成分の変更を行えばよいことが公知技術として知られている。［以下、これを『従来例１』という］。
また、従来例２として特公平６−３８２１２号がある。これは、電動機の軸に、回転速度検出器及び回転角検出器が結合されると共に減速歯車が結合され、該減速歯車の出力軸がトルク伝達機構を介してアームを駆動するような構造を有する産業用ロボットと、前記アームの加速度指令値及び位置指令値を発生する手段と演算部であって、前記発生手段から加速度指令値、速度指令値及び位置指令値から、電動機出力トルクの指令値と、電動機回転角速度の指令値との和を算出すると共に、電動機回転速度の指令値を算出する前記演算部と、前記回転角度の指令値から回転角度検出器によって検出される電動機回転変位の出力を減算する第１減算器と、電動機電流指令値を導出する第２減算器と、前記電動機の回転子、減速歯車、トルク伝達機構およびアームによって構成される軸系をばねと剛体とから成る系と仮定し、電動機電流指令値、電動機回転変位、及び電動機回転速度から、前記ばねと剛体から成る系のねじれ角度の推定値と、そのねじれ角度の時間微分値の推定値とを求める状態観測器と、前記ねじれ角の推定値と前記時間微分値の推定値とを加算する加算器とを含み、第２減算器は、第１減算器の出力と、演算部からの電動機出力トルクの指令値と電動機回転角速度の指令値との前記和とを加算し、その加算値から、回転角度検出器の出力と前記加算器の出力とを減算して前記電動機電流指令値を導出して電動機に与えることを特徴とする物体の位置制御装置である。
【０００３】
【発明が解決しようとする課題】
ところが、従来例１において、短い時間間隔でそのときどきの重力モーメント値とバネ定数を計算して、その重力モーメント値とバネ定数に応じて推定行列を計算し直すと、計算量が膨大になり、演算時間が不足する。新たにオブザーバ制御演算機構を現状のディジタル制御装置に組み込む場合など、演算時間の余裕がなく、従来技術では事実上、実現不可能であった。
また、従来例２は、重力モーメント変動などのダイナミクスを考慮した制御方式であるが、従来例１で述べたような相当量の演算を必要とし、設備の膨大化は避けられないという欠点があった。
本発明では、現実的な産業用ロボット等に適用して有効適切な手法として、オブザーバ推定行列計算時の離散化手段を巧みに駆使し、各従来例の問題点を払拭したオブザーバ制御演算装置を提供することを目的とする。
【０００４】
【課題を解決するための手段】
上記課題を解決するために、本発明は、２軸以上の複数の制御軸を有する制御対象に対し、各軸回りの重力モーメントを計算する演算機構と、各軸のバネ定数を計算する演算機構と、バネ定数計算値を用いて制御系の状態変数を推定するためのオブザーバ推定行列を計算する演算機構と、オブザーバ推定行列計算値の転送手段と、オブザーバ推定行列計算値を用いて状態変数の推定を行うオブザーバ演算機構を持つディジタル制御装置において、オブザーバ推定行列計算時の離散化を状態方程式の段階で１次で終了させるオブザーバ制御演算装置である。また好ましくは
オブザーバ制御演算装置において、各軸のバネ定数は、重力モーメント値の有理関数で近似し、必要精度に応じて代表点を増減して演算する前項に記載のオブザーバ制御演算装置である。さらに望ましくは
オブザーバ制御演算装置において、オブザーバ演算機構は位置・速度ループ演算機構内にあって、位置、速度ループのサンプリング時間と独立にサンプリング時間を設定する前項または前々項に記載のオブザーバ制御演算装置である。さらにまた好ましくは
オブザーバ制御演算装置において、コントローラ上位側でロボットの手先目標値から、各軸の関節角指令値を求め、これをコントローラ下位側に転送し、各軸の位置、速度のフィードバック制御を行うディジタル制御装置に、オブザーバ制御演算装置を接続し、オブザーバ制御演算装置の上位側では、ロボットアームの各部に設けられた質点の重さと回転軸からの距離、各軸の関節角をもとに重力モーメント演算機構で重力モーメント値を求め、その重力モーメント値を使ってバネ定数演算機構でバネ定数を求め、重力モーメント値とバネ定数をもとにオブザーバ推定行列を求め、そのオブザーバ推定行列をオブザーバ制御演算装置の下位側に転送すると共に、オブザーバ制御演算装置の下位側では、上位側から転送されたオブザーバ推定行列とコントローラ下位側からモータの速度検出信号とモータへのトルク指令値を受け取り、位置・速度ループのサンプリング時間とは独立に、ねじれ角及び負荷速度の推定を行い、それぞれのフィードバックゲインと乗算し、位置・速度ループから出力されたトルク指令値から減算するこれまでのいずれかの項に記載のオブザーバ制御演算装置である。なおかつ望ましくは
制御対象は、ロボットであるこれまでのいずれかの項に記載のオブザーバ制御演算装置である。
【０００５】
【作用】
本発明は、上記したオブザーバ演算手段により、推定精度をできるだけ落とさずに、しかも演算時間を短くでき、制振効果の安定性を高くできるオブザーバ制御演算装置を得ることができる。
【０００６】
【実施例】
以下、本発明の各実施例を図面に基づいて説明する。なお、各図面において、同一符号は同一もしくは相当部材を表す。
図１は、本発明の一実施例としてのオブザーバ演算装置を使用したディジタル制御装置の構成を示すブロック図である。
図１で使用した記号は次の［数１］通りとし、右上の添字は時間微分の回数を示すものとする。
【数１】
0003599127.tif 000002
【０００７】
このロボットのディジタル制御装置での一実施例では、コントローラ上位側２ａでロボット１の手先目標値設定手段２１で設定された手先目標値２１１，２１２，…，  ２１  ｎを、逆変換手段２２においてこれらを逆変換し、各軸の関節角指令値２２１（θ１），２２２  （θ２），  …，  ２２ｎ  （  θｎ  ）  を求め、それらをコントローラ下位側２ｂに転送し、各軸の位置、速度のフィードバック制御を行うディジタル制御装置２に、オブザーバ制御演算装置３を接続して構成されている。
すなわち、オブザーバ制御演算装置３の上位側では、ロボットアームの各部に設けられた質点の重さと回転軸からの距離、および各軸の関節角指令値２２１（θ１），２２２  （θ２），  …，  ２２ｎ  （  θｎ  ）  を基に、重力モーメント演算機構３１で各軸毎の重力モーメント値Ｍｇ１，  Ｍｇ２，  …，  Ｍｇ  ｎ  を求め、そのＭｇを使ってバネ定数演算機構３２でバネ定数Ｋｓを求め、重力モーメント値Ｍｇとバネ定数Ｋｓを使ってオ
ブザーバ推定行列演算機構３３でオブザーバ推定行列Ｊ，Ｆ，Ｇ，Ｌを求める。
そのオブザーバ推定行列Ｊ，Ｆ，Ｇ，Ｌを転送手段を介して、オブザーバ制御演算装置３の下位側に転送する。
オブザーバ制御演算装置３の下位側では、上位側から転送されたオブザーバ推定行列Ｊ，Ｆ，Ｇ，Ｌとコントローラ下位側から速度信号２８１，２８２，…，  ２８ｎ  とトルク指令２９１，２９２，…，  ２９ｎ  を受け取り、位置・速度ループ２４のサンプリング時間とは独立に、ねじれ角の推定３４ａ  及び負荷速度の推定３４ｂ  を行い、それぞれのフィードバックゲインＫωｓ  及びＫθＬ  と乗算し、位置・速度ループから出力されたトルク指令値２９から、減算器２５で減算した値とトルク定数３０を乗算することで、各軸の電流指令Ｉ１，Ｉ２，…，  Ｉｎ  となる。
なお、２６は位置信号２７から速度信号２８への変換手段［図１はｎ軸について表しているが第１軸から（ｎ−１）軸まで同様］である。また、重力モーメント演算に関節角指令値２２１（θ１），２２２  （θ２），  …，  ２２ｎ    （θｎ  ）  を用いているが、位置信号２７１，２７２  …，  ２７ｎ  ［図１で点線矢印で示す］を用いてもよい。
【０００８】
具体的に、数式を用いて説明する。以下使用する記号は次の［数２］通りとする。
【数２】
0003599127.tif 000003
この連続系の状態方程式に対し、離散化を施すと、次の［数３］ようになる。
【数３】
0003599127.tif 000004
一般的にはＡｄ、Ｂｄの値が十分収束するようにＮを大きくとるが、本発明では、Ｎ＝１で離散化を終了させることによって、Ａｄ、Ｂｄを次の［数４］ように求める。
【数４】
0003599127.tif 000005
この離散化状態方程式に対し、例えば「線形制御系の設計理論（計測自動学会編；１９７８）」で述べられているように最小次元オブザーバを設計すると、次の［数５］ようになる。
【数５】
0003599127.tif 000006
【０００９】
ここで使用されている行列Ｊ，Ｆ，Ｇ，Ｌは図１のものと同じである。具体的に、Ｊ，Ｆ，Ｇ，Ｌの行列成分を書き出すと、次の［数６］ように、バネ定数や重力モーメントに無関係な行列成分（Ｆ１１，  Ｆ１２，  Ｆ２１，  Ｆ２２）とそうでないものに分けることができる。バネ定数や重力モーメントに無関係な行列成分は予め計算しておくことと、リアルタイム演算の必要な部分でさえも、非常に簡単な数式であることから、演算負荷を軽減したオブザーバ推定行列を得ることができる。
次にバネ定数の計算について述べる。予めいくつかの重力モーメント値（Ｍｇ１，  Ｍｇ２，  …，  Ｍ  ｇｍ  ）におけるバネ定数（Ｋｓ１，  Ｋｓ２，  …，  Ｋ  ｓｍ  ）を実験等で求めておく。ロボットの実動作時には、逐次計算される重力モーメント値をもとに、例えば次の［数７］ような１次のラグランジェ補間を行うことで、そのときどきの重力モーメント値におけるバネ定数を求める。
【数７】
0003599127.tif 000007
【００１０】
図３のように、得られたバネ定数列（Ｋｓ１，  Ｋｓ２，  …，  Ｋ  ｓｍ  ）でバネ定数曲線を多項式近似する際、代表点数ｍの数［この図３では代表点数を４点］によっては精度の悪い近似となってしまうことがあり、制振効果が不安定になる問題を引き起こす。
本発明では、バネ定数演算機構に、得られたバネ定数曲線の形に応じて、代表点数をパラメータで可変にする構造を持たせる。これによって、本発明においては図４のようにバネ定数曲線［この図４では代表点数を８点］を高い精度で近似できる。
【００１１】
以上の方法で、コントローラ上位側の演算周期毎に得られる各軸毎のオブザーバ推定行列（Ｊ，Ｆ，Ｇ，Ｌ）を所定の方法でコントローラ下位側が受け取り、コントローラ下位側で状態推定演算を行う。その場合、すでに位置・速度ループ演算や各種アラーム検出処理のために、コントローラ下位側のＣＰＵがサンプリング時間内で使用できる演算時間が短く、同じサンプリング周期ではオブザーバの演算が終了できない場合がある。
このような問題に対し、本発明では位置・速度ループのサンプリング時間とは独立にオブザーバ演算用のサンプリング時間を設け、コントローラ下位側ＣＰＵの僅かな空き時間に少しづつオブザーバ演算を行う。これを図５のタイムチャートに示す。
本発明の一実施例におけるタイムチャートを表す図５の中で、ｉは位置・速度のサンプリング時間、Ｐ（ｉ），Ｐ（ｉ＋１），…は位置・速度ループ演算や各種アラーム検出処理に使用する時間、ｋはオブザーバのサンプリング時間、Ｏ（ｋ）ｉ，Ｏ（ｋ）ｉ＋１，…はオブザーバ演算に使用する時間を示す。この例では、位置・速度ループの５サンプリング時間がオブザーバの１サンプリング時間となっている。式（１３）、（１４）、（１５）をそれぞれのＯ（ｋ）に合わせて分割して演算させる。
このような演算時間の操作から、極めて短い時間で、オブザーバ推定行列（Ｊ，Ｆ，Ｇ，Ｌ）をオブザーバ推定行列演算機構３３で算出でき、さらにオブザーバ演算機構３４でねじれ角および負荷速度が、より真値に近くして、演算推定可能となる。
【００１２】
【発明の効果】
以上説明したように本発明では、２軸以上の複数の制御軸を有する制御対象に対し、各軸回りの重力モーメントを計算する演算機構と、各軸のバネ定数を計算する演算機構と、前記バネ定数計算値を用いて制御系の状態変数を推定するためのオブザーバ推定行列を計算する演算機構と、前記オブザーバ推定行列計算値の転送手段と、オブザーバ推定行列計算値と重力モーメント計算値を用いて状態変数の推定を行うオブザーバ演算機構を持つディジタル制御装置において、オブザーバ推定行列計算時の離散化を状態方程式の段階で１次で終了させ、各軸のバネ定数は、重力モーメント値の有理関数で近似し、必要精度に応じて代表点を増減して演算し、オブザーバ演算機構は位置、速度ループのサンプリング時間と独立にサンプリング時間を設定することにより、
推定精度をできるだけ落とさずに演算時間を短くでき、制振効果の安定性を高くできるオブザーバ制御演算装置を得ることが可能であるという特段の効果を得ることができる。
例えば、産業用ロボットのような非線形な制御対象に適用すれば、実用的なオブザーバによる推定状態フィードバック制御を行うことができる。
【図面の簡単な説明】
【図１】本発明のオブザーバ制御演算装置を含むロボットコントローラの一実施例の回路構成を示すブロック図
【図２】ロボット動特性モデルの説明図
【図３】バネ定数曲線の粗い近似例［従来例］
【図４】本発明によるバネ定数曲線の高精度な近似例
【図５】図１における実施例のコントローラ下位側ＣＰＵのタイムチャート
【符号の説明】
１  ロボット
２  ディジタル制御装置
２ａ  コントローラ上位側
２ｂ  コントローラ下位側
２１  ロボットの手先目標値設定手段
２２  逆変換手段
２３１    第１番目のフィードバック制御
２３２  第２番目のフィードバック制御
２３ｎ    第ｎ番目のフィードバック制御
２４    位置・速度ループ
２５    減算器
２６    位置信号を速度信号に変換する変換手段
２７  位置信号
２８  速度信号
２９  トルク指令
３    オブザーバ制御演算装置
３０  トルク定数
３１  重力モーメント演算装置
３２  バネ定数演算機構
３３  オブザーバ推定行列演算機構
３４  オブザーバ演算機構
【数６】0003599127.tif 000008
【図１】
0003599127.tif 000009
【図２】
0003599127.tif 000010
【図３】
0003599127.tif 000011
【図４】
0003599127.tif 000012
【図５】
0003599127.tif 000013