(57)【特許請求の範囲】
【請求項１】
ロボットを直交座標系で直線動作あるいは円弧動作のためにサンプリング周期ごとに移動係数の加算を行い目標位置を求める補間演算部と、
該目標位置に対応する関節座標系での関節座標データに変換する逆変換部と、
前記サンプリング周期ごとの関節座標データとの差分である関節移動データを各軸について求め出力する関節移動データ作成出力部であって、既出力された前記関節移動データの推移に基づいて次回に出力する関節移動速度を各軸について推測する手段を有する産業用ロボットの制御装置において、
前記推測した速度が各軸関節ごとに予め設定した所定値を超える場合はその所定値を前記推測速度で除して補正係数を求める手段を備え、
前記補間演算部は、次回サンプリング周期の補間演算時には前記移動係数に前記補正係数を乗じた補正移動係数の加算を行い前記目標位置を求め、前記ロボットが終点位置に到達する最終回のサンプリング周期の移動係数あるいは補正移動係数の加算時に、加算結果が最終値となるように補正を行なうこと特徴とする産業用ロボットの制御装置。
【請求項２】
前記補正係数を求める場合に前記推測速度が各軸関節ごとに前記所定値を最も大きく超える軸関節についての補正係数を求めることを特徴とする請求項１記載の産業用ロボットの制御装置。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、産業用ロボットの制御装置に関するもので、さらに詳しく言えば、特異点を通過できる補間演算制御機能を有する産業用ロボットの制御装置に関するものである。
【０００２】
【従来の技術】
多くの多関節ロボットは、各関節が回転機構で構成される。この場合、各関節角の回転だけで所定の位置から所定の位置へと直線や円弧等の補間動作を姿勢を含めて制御することは図２に示すように行われる。
すなわち、教示データ記憶部１に記憶されている関節座標系の教示位置を順変換部２で直交座標系に座標変換（順変換）し、補間演算部３で直交座標系で直線や円弧等の軌跡となるように動作速度を考慮しサンプリング周期毎の目標となる位置と姿勢を演算し、逆変換部４で目標となる直交座標系での位置と姿勢を関節座標系に座標変換（逆変換）し、最後にセグメントデータ作成出力部５で前回の目標位置に対する関節座標データと差分（セグメントデータと称す）を作成して、サーボ駆動系６に出力することで補間動作を実現している。
補間演算の基本式を式（１）に示す。
Ｐ(k) ＝Ｐs ＋（ｋ／Ｎ）＊（Ｐe −Ｐs ）  …（１）
ただし、Ｐs は始点位置を表すベクトル、Ｐe は終点位置を表すベクトル、Ｐ(k) はＰs からＰe 間の任意の位置を表すベクトル、ＮはＰs からＰe まで指定された動作速度で移動させる場合に必要なサンプリング回数であり、動作速度、サンプリング周期および移動距離から求められる。k は整数であり、サンプリング周期毎にｋを０からＮまで１づつ加算させることによって、つまり、サンプリング周期毎に（１／Ｎ）＊（Ｐe −Ｐs ）を出力することにより、Ｐ(k) が始点Ｐs から終点Ｐe までの間を指定された動作速度で変化することになる。
【０００３】
ところで、多自由度多関節ロボットにおいては逆変換時に複数の解があることが知られている。補間動作を行う場合に、適宜に解を選択して移動させると、途中で特異点、およびその近傍を通過する場合があり、指定された動作速度で補間演算を行うと、図３のＢに示すように各軸の関節速度が最大速度を超えて動作不可能になってしまう場合（以下、セグメントオーバと称す）がある。
これを解決するための手段として、従来は、特異点を検出し、特異点およびその近傍で演算アルゴリズムを切り換えることで通過させる方法で回避していた（例えば、特開昭６２−１８９５０４号公報参照）。
【０００４】
【発明が解決しようとする課題】
ところが、従来の方法では、特異点を検出するとアルゴリズムを変更し、特殊処理が必要となる。また、特異点近傍で動作可能か否かは、そのときの動作速度やロボットの位置等で条件が変わるので確実に各軸が動作可能か否かを判定するのは困難である。さらに、例えばアームが座標原点に近い場所で補間動作を行う場合のように、特異点以外にも同様に各軸の関節速度が最大速度を超えて動作不可能になる場合があるが、これを回避するためにはまた別のアルゴリズムが必要となる。
本発明の目的は、直線、円弧等の補間演算での特異点近傍やその他の位置で発生する可能性のある最大速度を超えて動作不可能になる場合を、簡単に、かつ統一的な手法で回避できるロボットの制御方法を提供することである。
【０００５】
【課題を解決するための手段】
本発明は、ロボットを直交座標系で直線動作あるいは円弧動作のためにサンプリング周期ごとに移動係数の加算を行い目標位置を求める補間演算部と、該目標位置に対応する関節座標系での関節座標データに変換する逆変換部と、前記サンプリング周期ごとの関節座標データとの差分である関節移動データを各軸について求め出力する関節移動データ作成出力部であって、既出力された前記関節移動データの推移に基づいて次回に出力する関節移動速度を各軸について推測する手段を有する産業用ロボットの制御装置において、前記推測した速度が各軸関節ごとに予め設定した所定値を超える場合はその所定値を前記推測速度で除して補正係数を求める手段を備え、前記補間演算部は、次回サンプリング周期の補間演算時には前記移動係数に前記補正係数を乗じた補正移動係数の加算を行い前記目標位置を求め、前記ロボットが終点位置に到達する最終回のサンプリング周期の移動係数あるいは補正移動係数の加算時に、加算結果が最終値となるように補正を行なうこと特徴とするものである。
また、前記補正係数を求める場合に前記推測速度が各軸関節ごとに前記所定値を最も大きく超える軸関節についての補正係数を求めることを特徴とするものである。
【０００６】
【作用】
これにより、推測されたセグメントデータが最大値を越える場合は、前記補正係数が乗算されるので次回演算時では最大値内におさまる。
【０００７】
【実施例】
図１に、本発明の実施例を示して説明する。
本発明は補間演算部３を改良し、セグメントデータ記憶部７、セグメントオーバ推測部８、および補正係数演算部９を追加したものである。
補間動作を開始すると、順変換部２から、補間演算部３、逆変換部４、セグメントデータ作成出力部５を経て補間演算を行いセグメントデータと作成する。次にセグメントデータ記憶部７によって、セグメントデータ作成出力部５で作成したセグメントデータを記憶する。次にセグメントオーバ推測部８によって、セグメントデータ記憶部７で記憶した過去に出力したセグメントデータを利用して次回に出力するセグメントデータを推測する。補正係数演算部９では、前記推測データが予め設定した値を超える場合は、その所定値を前記推測データで除して補正係数を求めるその係数を計算する。次回演算周期では、補間演算部３によって補正係数演算部９で求めた係数を用いて、最も動作速度の大きい軸の関節速度がセグメントオーバとならないように補間動作速度を推測した係数を乗じて補間演算する。
【０００８】
直線補間の場合は、順変換部２により始点および終点をあらかじめ順変換しておき、式（１）で直交座標でのＰ(k) を求め、それを逆変換部４で逆変換することによってＰ(k) の位置に相当する関節座標値を得る。また、セグメントデータ作成出力部５で前回求められたＰ(k-1) に相当する関節座標値との差によってセグメントデータが得られる。
セグメントデータ記憶部７では、セグメントデータ作成出力部５で求められたセグメントデータをサンプリング周期毎に記憶する。記憶するステップ数は任意ステップが可能である。また、記憶方法については、リングバッファ等の手段がある。
【０００９】
セグメントオーバ推測部８では、セグメントデータ記憶部７で記憶したセグメントデータを利用して次回の補間演算でのセグメントデータを推測する。ここでは説明を簡単にするため、具体例として過去２回分の１軸のみのセグメントデータを用いて、推測する方法の一例を述べる。
最大関節速度に相当するセグメントデータを１００、前回のセグメントデータが６０、今回のセグメントデータが９０であったとする。前回と今回の差分が３０であることから、次回は９０＋３０＝１２０が出力されるものと推測する。
推測データ１２０が最大値１００を越えたため、補正係数演算部７では、次回補間演算で１００を超えないような補正係数Ａを以下のように求める。
Ａ＝１００／１２０＝０．８３３  …（２）
この補正係数Ａとは、従来例でK を整数とし、０からＮまで１ずつ加算していたが、その「１」に相当するものである。つまり、本発明では、次回指令が最大値を超える場合は、K は整数にはならず、１ではなくＡを加算するのである。なお、１ではなくＡを加算するから、このままでは最終値（合計）がＮにならないので、最終回の演算で補正を行えばよい。
以上の動作により、図３のＡに示すように、最大値を超えることなく動作するようになる。
【００１０】
上記説明では補正係数Ａを説明のため過去２回分のセグメントデータを用いて変化量が一定つまり直線的に変化するものとして求めたが、より多くの過去のセグメントデータを用いて、種々の関数を用いて推測することも可能である。また複数軸の場合は、最大関節速度を最も超えるものを選択すればよい。
サンプリング周期毎において、式（１）で補間演算するが、前述のｋを０からＮまで補正係数Ａを加算することでセグメントオーバとなることなく最大関節速度で前述の特異点近傍等を通過することができる。
また、直線補間に限らず種々の補間にも適用できることは勿論である。
【００１１】
【発明の効果】
以上説明したように本発明は、過去に演算した各関節のセグメントデータを直接的に用いて推測するため、特異点近傍のみならず全ての動作範囲において補間動作時のセグメントオーバによる動作不能状態を回避することができる。また、特異点の位置などを事前に検出しておく必要もなくなる。
【図面の簡単な説明】
【図１】本発明の実施例を示す図
【図２】従来例を示す図
【図３】本発明と従来の動作例を説明する図
【符号の説明】
２…順変換部、３…補間演算部、４…逆変換部、５…セグメントデータ作成出力部、７…セグメントデータ記憶部、８…セグメントオーバ推測部、９…補正係数演算部
【図１】
0003748454.tif 000002
【図２】
0003748454.tif 000003
【図３】
0003748454.tif 000004