力制御装置
【課題】動作中に力制限値とコンプライアンス制御パラメータを切替えることで作業を高速化しながら位置ずれ発生時に過大な力が作用するのを防止した力制御装置を提供する。
【解決手段】切替地点指定手段５からのロボットに行わせる作業動作に従ったロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータ条件、および上記パラメータ条件に切り替える切替地点を指示する命令に基づいて、パラメータ切替手段６が力制限値および力制御パラメータの少なくとも一方を切り替え、力制限超過判別手段４が力制限値を超える力が作用していると判別した場合に、ロボットに行わせる作業動作に従ってロボットの目標位置を示す位置指令を出力する指令生成手段１にロボットを減速停止させる位置指令を出力させる停止指令を入力する。
【解決手段】切替地点指定手段５からのロボットに行わせる作業動作に従ったロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータ条件、および上記パラメータ条件に切り替える切替地点を指示する命令に基づいて、パラメータ切替手段６が力制限値および力制御パラメータの少なくとも一方を切り替え、力制限超過判別手段４が力制限値を超える力が作用していると判別した場合に、ロボットに行わせる作業動作に従ってロボットの目標位置を示す位置指令を出力する指令生成手段１にロボットを減速停止させる位置指令を出力させる停止指令を入力する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は組立、バリ取り、研磨、基板実装、検査などを行うロボット、自動組立装置、実装機、加工機、検査装置などに適用される力制御装置に関する。
【背景技術】
【０００２】
例えば下記特許文献１に記載のこの種の装置では、部品を収納したマガジンから部品を取り出し、所定位置まで移動して部品組み付け作業を行うロボットの先端部に力センサが設けられている。ロボットコントローラは、力センサで検出した力のフィードバックによるコンプライアンス制御を行い、検出した力と予め設定された力の閾値を比較して、その比較結果により次のロボット動作の決定、作業成否の判定を行っている。例えば丸棒状の部品をハンドで把持して下降して挿入を行う場合、挿入中(下降中)に検出した力が閾値を超えた場合は、挿入作業が失敗したとして下降動作を停止することにより、過大な力の発生を防止する。ハンド停止後は再び作業を行うか、あるいは、異常警報を発生する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平７−２４６６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
コネクタやゴムホースの挿入作業等のように、作業開始時は反力が小さいが作業終了時には反力が大きくなる作業が多くある。こうした作業を１つの動作で行う場合、従来の装置では、コンプライアンス制御のパラメータや力の閾値は、作業終了時に作用する大きな反力に適した値に設定する必要があった。そのため作業開始時に位置ずれがある場合でも、必要以上に大きな力の閾値を設定する必要があるため、位置ずれ時に発生する力を小さくするには作業速度を落としておく必要があり、動作時間が長くなる問題があった。
【０００５】
また検出した力が予め設定した閾値を超過するかどうかの判別は行うが、閾値を超過していない方向の力情報を活用していないため、位置ずれ時に作業エラーを発生して停止させることはできるが、その後の復旧動作については何等考慮がなされていなかった。
【０００６】
この発明は、上記の課題を解決するためになされたものであり、動作中に力閾値(力制限値)とコンプライアンス制御パラメータを切替えることで、作業を高速化しながら、位置ずれ発生時に過大な力が作用することを防止した力制御装置を提供することを得ることを目的とする。
【課題を解決するための手段】
【０００７】
この発明は、切替地点指定手段からのロボットに行わせる作業動作に従ったロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータを切り替える切替地点を指示する命令に基づいて、パラメータ切替手段が力制限値および力制御パラメータの少なくとも一方を切り替え、力制限超過判別手段が力制限値を超える力が作用していると判別した場合に、ロボットに行わせる作業動作に従ってロボットの目標位置を示す位置指令を出力する指令生成手段にロボットを減速停止させる位置指令を出力させる停止指令を入力することを特徴とする力制御装置にある。
【発明の効果】
【０００８】
この発明では、動作中に力閾値(力制限値)とコンプライアンス制御パラメータを切替えることで、作業を高速化しながら、位置ずれ発生時に過大な力が作用することを防止することができる。
【図面の簡単な説明】
【０００９】
【図１】この発明の実施の形態１、２における力制御装置の構成を示すブロック図である。
【図２】図１のロボット制御手段の内部構成の一例を示す図である。
【図３】図２の各軸位置制御手段の内部構成の一例を示す図である。
【図４】図２の力覚考慮補正手段の内部構成の一例を示す図である。
【図５】図２の力覚考慮補正手段の内部構成の別の例を示す図である。
【図６】図２の力覚考慮補正手段の内部構成のさらに別の例を示す図である。
【図７】図２の力覚考慮補正手段の内部構成のさらに別の例を示す図である。
【図８】この発明の実施の形態３における力制御装置の構成を示すブロック図である。
【図９】図８の力覚考慮補正手段の内部構成の一例を示す図である。
【図１０】この発明の実施の形態４における力制御装置の構成を示すブロック図である。
【図１１】この発明の実施の形態５〜９における力制御装置の構成を示すブロック図である。
【図１２】この発明の実施の形態１０における力制御装置の構成を示すブロック図である。
【図１３】この発明の実施の形態１１における力制御装置の構成を示すブロック図である。
【発明を実施するための形態】
【００１０】
以下、この発明による力制御装置を各実施の形態に従って図面を用いて説明する。なお、各実施の形態において、同一もしくは相当部分は同一符号で示し、重複する説明は省略する。
【００１１】
実施の形態１．
図１はこの発明の実施の形態１における力制御装置の構成を示すブロック図である。指令生成手段１は例えば記憶手段Ｍに予め格納されているロボット３に行わせる作業の作業動作プログラムに基づいてロボット制御手段２にロボット３の時々刻々の目標位置を示す位置指令を入力する。ロボット制御手段２は与えられた位置指令および後述する例えばコンプライアンス制御の力制御パラメータ、ロボット３からのフィードバック信号に従ってロボット３を追従制御する。
【００１２】
ロボット制御手段２により制御されるロボット３は、１軸もしくは複数軸のモータ(図示省略)を備えるものであり、その一例として産業用ロボットと呼ばれる６軸垂直多関節ロボット、４軸水平多関節ロボット等がある。ロボット３の先端部近辺もしくはロボットが作業する対象には作用する力とモーメントを計測するための力覚センサが取り付けられており(共に図示省略)、力覚センサで測定された力及びモーメントは力検出値としてロボット制御手段２および力制限超過判別手段４に入力される。
【００１３】
力検出値は６軸の力覚センサを用いている場合は３軸方向の力と各軸周りのモーメントであり、５軸以下の力覚センサを用いている場合は、３軸方向の力と各軸周りのモーメントのうち、計測できるモーメントであり、各力覚センサがこれらのデータをロボット制御手段２及び力制限超過判別手段４に送信する。またロボット３を駆動する各軸のモータにはエンコーダやレゾルバなどの各軸のモータ位置を測定するセンサが取り付けられており(図示省略)、こうしたセンサで測定した各軸のモータ位置はロボット制御手段２に送信される。
【００１４】
力制限超過判別手段４では、パラメータ切替手段６から入力される力閾値(力制限値)とロボット３の力覚センサから送信される力検出値を比較し、少なくとも１つの軸方向の力もしくはモーメントが力閾値を超えた場合に、指令生成手段１に停止指令を入力する。この実施の形態では力制限超過の判別を検出した力とモーメントの各成分に対して実施するが、合成した力やモーメントに対して実施してもよい。指令生成手段１は力制限超過判別手段４から停止指令を受け取ると、現在の位置・速度から減速停止する指令を生成してロボット制御手段２に入力する。
【００１５】
切替地点指定手段５は、上記作業動作プログラムの動作に従ってパラメータ切替手段６でパラメータを切替える地点と切替後のパラメータを順次指定する。パラメータ切替手段６で切替えるパラメータが力閾値と力制御パラメータの２種類の場合、例えばロボット３の動作を記述するロボットプログラム内で下記のような命令を実行して指定する。
【００１６】
FCNT ３０，２
【００１７】
ここで、「FCNT」は切替地点を指定するためのロボット言語(命令)で、「３０」は動作開始から３０％の地点でパラメータの切替を開始することを指定し、「２」は切替後のパラメータ条件番号である。条件番号ごとに力閾値と力制御パラメータの組合せが例えば記憶手段Ｍに記録されており、パラメータ切替手段６は位置指令が切替地点指定手段５で設定された地点に到達した段階で、指定された条件番号に対応する力閾値と力制御パラメータを読み出す。
【００１８】
なお図１において、ロボット３を除く指令生成手段１、ロボット制御手段２、力制限超過判別手段４、切替地点指定手段５、パラメータ切替手段６は例えば１つのコンピュータ(ロボットコントローラ)により構成され、記憶手段Ｍは各手段で共有する該コンピュータ内のメモリ(ＲＯＭ、ＲＡＭ等を含むものとする)を示す。
【００１９】
パラメータ切替手段６では、力閾値に関しては、指定された切替地点通過後に直ちに切替える。力制御パラメータに関しては、切替前の力制御パラメータから切替後の力制御パラメータへ例えば２段の移動平均フィルタを介した出力で切替えを行う。切替の開始を指定された切替地点を通過時点とする。力制御パラメータの切替は１段もしくは３段以上の移動平均フィルタを用いてもよいし、移動平均以外の、例えば１次遅れフィルタなどを用いてもよい。また、切替前の力制御パラメータから切替後の力制御パラメータまで切替える関数を内部に備えておき、備えてある関数を用いて徐々に切替えてもよい。また、この実施の形態では動作開始地点からの移動距離の割合で切替開始地点を指定したが、動作終了地点までの移動距離の割合、動作開始地点からの移動距離、動作終了地点までの移動距離、動作開始からの経過時間などで指定してもよい。
【００２０】
ロボット制御手段２の内部構成の一例を図２に示す。指令生成手段１で生成した位置指令を力覚考慮補正手段７の出力を加算部Ｋ１で加算することで補正する。逆変換手段８は、手先位置姿勢を実現する各軸のモータ位置を算出する逆変換計算を行い、力覚考慮補正手段７の出力を加算することで補正した位置指令をロボットの各軸の位置指令である関節位置指令に変換する。変換した関節位置指令は各軸位置制御手段９に入力される。各軸位置制御手段９は入力された関節位置指令に各軸のモータ位置が追従するように制御を行う。
【００２１】
各軸位置制御手段９の内部構成の一例を図３に示す。図３の各軸位置制御手段９のモータ毎に設けられた各軸位置制御ユニット９ｎでは、各軸の(関節)位置指令と当該軸のモータ位置の差を減算部Ｇ１で求めて比例部２１に入力する。そして比例部２１の出力と微分部２３で求めたモータ位置の微分値から減算部Ｇ２で算出したモータ速度の差を求め、求めた差を比例積分部２２に入力する。上記制御系は位置比例制御系の内部に速度比例積分制御系がある構成になっており、モータの位置制御系としてはよく使われる構成である。
【００２２】
なお、この実施の形態では各軸位置制御ユニット９ｎの構成として、各軸毎にモータ位置制御と速度制御を行うフィードバック制御系の構成としたが、各軸ごとにオブザーバを含む制御系であってもよいし、フィードフォワードを含む制御系であってもよい。また、全て各軸毎に独立した構成ではなく、軸間の干渉トルクをフィードフォワードもしくはフィードバックで補正する構成であってもよい。
【００２３】
次に、力覚考慮補正手段７の内部構成の一例を図４に示す。力覚考慮補正手段７の内部では、まず順変換部１８において各軸モータ位置からそのときの手先位置姿勢を算出する順変換を行い手先位置姿勢を算出し、指令生成手段１で生成した位置指令(手先位置姿勢の指令)と順変換部１８で算出した手先位置姿勢の差を減算部Ｇ３で算出し、算出した差をツール座標系にツール系座標変換部１０により変換する。ツール座標系とは、ロボット先端に取り付けたツールやハンドに固定された座標系である。
【００２４】
ツール系座標変換部１０の出力をスティフネス行列部１１に入力し、スティフネス行列とツール系座標変換部１０の出力であるベクトルの積を求め、その積をスティフネス行列部１１の出力とする。さらにツール系座標変換部１０の出力の各要素の微分値を微分部１５で算出し、算出した微分値から構成されるベクトルとダンピング行列の積をダンピング行列部１６の出力とし、スティフネス行列部１１とダンピング行列部１６の出力を加算部Ｋ２で加算する。
【００２５】
次に、ロボット３の力覚センサで検出した力検出値をツール系座標変換部１７でツール座標系に変換し、変換した力検出値を先ほどの加算部Ｋ２の出力結果から減算部Ｇ４で減算する。減算した結果の各要素にパラメータ切替手段６で切替えられる可変の比例ゲイン値を可変ゲイン部１２で乗じ、その後、直交系座標変換部１３で直交座標系への座標変換を行い、積分部１４で積分を行ってから位置指令修正量(位置指令補正量)としてロボット制御手段２の加算部Ｋ１に入力する。可変ゲイン部１２にはパラメータ切替手段６で切替える力制御パラメータが使用される。
【００２６】
図４では、力制御系のパラメータとして力検出値との差に対する比例ゲインのみを変更する場合を例に挙げて説明したが、力覚考慮補正手段７は図５に示すように、スティフネス行列部１１及びダンピング行列部１６にそれぞれパラメータ切替手段６で切替える力制御パラメータを使用して各行列要素値を与え、スティフネス行列及びダンピング行列の各要素も指定された切替地点から切替えるようにしてもよい。
【００２７】
また、図６に示すように、スティフネス行列部１１及びダンピング行列部１６にそれぞれパラメータ切替手段６で切替える力制御パラメータを使用して各行列要素値を与え、スティフネス行列及びダンピング行列の各要素を指定された切替地点から切替、可変ゲイン部１２と積分部１４の代わりに比例積分部１９を設けて力検出値との差に乗じるゲインは切替えない構成にしてもよい。
【００２８】
また、ツール座標系にいったん変換してから位置指令修正量(位置指令補正量)を算出するのではなく、図７に示すように、ツール系座標変換部１０及び直交系座標変換部１３を設けず、ツール系座標変換部１７の代わりに直交系座標変換部２０を設けることで、直交座標系で位置指令修正量(位置指令補正量)を算出して補正してもよい。
【００２９】
またこの実施の形態では動作中の１箇所で力閾値及び力制御パラメータを切替える場合を例に挙げて説明したが、例えば
【００３０】
FCNT ３０，２，５０，３
【００３１】
と記載し、移動距離の３０％を越えた地点で条件番号２へ切替、移動距離の５０％を越えた地点で条件番号３へ切替えるなど、動作中の２箇所以上で切替える構成にしてもよい。また、パラメータ番号ではなく、切替えるパラメータの値を直接記載してもよい。
【００３２】
この実施の形態１によれば、動作中の切替地点をロボットを動作させるロボットプログラム中の指令で指定された地点で力閾値と力制御パラメータを切替えることができるため、動作中に指定した地点で力閾値と力制御パラメータの切替が可能となり、位置ずれ時の安全な停止と高速な挿入動作の両立が実現できる。
また、動作途中で力制御系のパラメータを変更するため、接触直後は柔らかくロボットを動作させ、押し込みや挿入の最終段階ではロボットを固く動作させることも可能となり、接触開始直後は柔らかくしてダメージを抑えながら、反力に対抗して押し込み力が必要とうなる作業を高速に実施することが可能となる。
また、プログラムの指令で動作直前に切替地点を指定できるため、動作ごとにきめ細かく設定することができる。
【００３３】
実施の形態２．
以下にこの発明の実施の形態２における力制御装置を説明する。装置の構成は基本的には図１に示すものと同じであるが、切替地点指定手段５の作用が異なる。例えば切替地点指定手段５からパラメータ切替手段６に送られる上述の命令のパラメータ(切替地点や条件番号)を、コンピュータに接続された手動操作盤またはパソコンからなる図１に破線で示す入力手段Ｉから値を書き換えるようにして指定する。
【００３４】
例えば上述のFCNTのパラメータ(ファクタ)の値をFCNT(３０，１，２)と設定すると、パラメータ切替手段６は動作開始からその動作の移動量の３０％を超過するまでは条件番号１で設定されている力閾値と力制御パラメータを力制限超過判別手段４及びロボット制御手段２に入力し、動作開始からその動作の移動量の３０％を超過後は、力閾値として条件番号２で設定されている値を力制限超過判別手段４に入力するとともに、ロボット制御手段２へ入力する力制御パラメータの値を徐々に条件番号２の値へ切替える。
【００３５】
この実施の形態２によれば、上記に加え、入力手段Ｉからの入力により動作直前に命令のパラメータを指定できるため、動作ごとにきめ細かく設定することができる。
【００３６】
実施の形態３．
図８はこの発明の実施の形態３における力制御装置の構成を示すブロック図である。実施の形態１とは異なり、パラメータ切替手段６では力閾値のみ切替える。したがって実施の形態１の図２に示されているロボット制御手段２の力覚考慮補正手段７の内部も、実施の形態３では図９に示すように可変ゲイン部１２の代わりに比例ゲイン部１２ａが設けられた固定パラメータの制御系となる。
【００３７】
この実施の形態３によれば、動作中の切替地点をロボットを動作させる指令ロボットプログラムの指令で指定された地点で力閾値を切替えることができるため、動作中に指定した地点で力閾値の切替が可能となり、位置ずれ時の安全な停止と高速な挿入動作の両立が実現できる。
【００３８】
実施の形態４．
図１０はこの発明の実施の形態４における力制御装置の構成を示すブロック図である。実施の形態１とは異なり、パラメータ切替手段６では力制御パラメータのみ切替える。
【００３９】
この実施の形態４によれば、動作途中で力制御系のパラメータを変更するため、接触直後は柔らかくロボットを動作させ、押し込みや挿入の最終段階ではロボットを固く動作させることも可能となり、接触開始直後は柔らかくしてダメージを抑えながら、反力に対抗して押し込み力が必要となる作業を高速に実施することが可能となる。
【００４０】
実施の形態５．
図１１はこの発明の実施の形態５における力制御装置の構成を示すブロック図である。実施の形態１では力制限値(力閾値)超過後、停止指令を指令生成手段１に入力していたが、実施の形態５では指令生成手段１に加えて復旧動作決定手段２４を設け、復旧動作決定手段２４にも停止指令を入力する。なおこの実施の形態では、図１１において、ロボット３の力検出値、各軸モータ位置のデータは復旧動作決定手段２４へは送られない。
【００４１】
復旧動作決定手段２４は、停止指令が入力されると、その時実行中の作業動作の動作開始地点までの退避動作を行わせるための指令生成命令を指令生成手段１に入力する。指令生成手段１は指令生成命令を受けると、作業動作プログラムより該動作の動作開始地点を求め、上記退避動作のための位置指令をロボット制御手段２へ入力する。
【００４２】
この実施の形態５によれば、力制限値(力閾値)を超過した場合に、ロボットの状態が自動的に、直前の動作開始地点まで戻ることにより、力制限値を超過した状態が継続することを防止できる効果がある。
【００４３】
実施の形態６．
この発明の実施の形態６における力制御装置の構成は基本的に図１１に示すものと同じである。復旧動作決定手段２４では、ロボット３から時々刻々の各軸モータ位置と力検出値を入力し、動作中に力検出値が最後に所定の規定値以下となった地点を各軸モータ位置として記憶しておく(例えば記憶手段Ｍに記憶)。そして力制限超過判別手段４から停止指令が入力されると、記憶してある最後に規定値以下となった地点まで戻る退避動作を行わせるための、規定値以下となった地点の各軸モータ位置を含む指令生成命令を指令生成手段１に入力する。指令生成手段１は指令生成命令を受けると、受け取った規定値以下となった地点の各軸モータ位置に従って、上記退避動作のための位置指令を、ロボット制御手段２へ入力する。
【００４４】
規定値は値そのものを予め設定(例えば記憶手段Ｍに記憶)しておいてよいし、力閾値との比率(例えば０．８倍)を設定し、常に力制限超過判別手段４で使用する力閾値(力制限値)に設定された比率(例えば記憶手段Ｍに記憶またはプログラム中に計算式として入れる)を乗じた値を規定値としてもよい。その場合、パラメータ切替手段６は図１１に破線で示すように、常に力閾値を復旧動作決定手段２４にも入力する。また、この実施の形態では力閾値を復旧動作決定手段２４が記憶してある各軸モータ位置に基づいて退避動作を行わせるが、モータ位置を記憶するのではなく、動作中に力検出値が最後に規定値以下となった時の指令生成手段１の出力する位置指令を記憶しておき、記憶してある位置指令の地点まで戻る動作を行わせてもよい。その場合、指令生成手段１は図１１に破線で示すように、常に位置指令を復旧動作決定手段２４にも入力する。復旧動作決定手段２４は規定値以下となった地点の位置指令を含む指令生成命令を指令生成手段１に入力する。
【００４５】
この実施の形態６によれば、力制限値(力閾値)を超過した場合に、ロボットの状態が自動的に、力が規定値以下となる地点まで戻ることにより、力制限を超過した状態が継続することを防止できる効果がある。また、動作開始地点までの退避動作に比べて、戻るときの移動量を少なくできるため、退避動作の時間短縮、また予想外の衝突などを防止できる効果もある
【００４６】
実施の形態７．
この発明の実施の形態７における力制御装置の構成は基本的に図１１に示すものと同じである。実施の形態７では、復旧動作決定手段２４が、停止指令が入力されたときの力検出値により、その後の動作を選択する。押し込み方向以外の力検出値が全て規定値以下の場合は、実施の形態６と同様に押し込み方向の力検出値が最後に規定値以下となった地点まで戻る退避動作を行わせる指令生成指令を指令生成手段１に入力する。
【００４７】
押し込み方向以外の力の少なくとも１つが規定値以上の場合は、停止した地点で探索動作を行わせる指令発生指令を指令生成手段１に入力し、ロボット３の動作を戻す方向に探索動作を行う退避動作を行わせる。探索動作の途中で押し込み方向の力が規定値以下となればその地点で停止させ、その後、元々の動作を再開する位置指令を発生させる指令発生指令(動作再開指令)を指令生成手段１に入力する。
【００４８】
この実施の形態７によれば、力制限値(力閾値)を超過した場合に、探索動作を行うことにより自動的に動作を復旧継続できる効果がある。
【００４９】
実施の形態８．
この発明の実施の形態８における力制御装置の構成は基本的に図１１に示すものと同じである。実施の形態８では、復旧動作決定手段２４は、力制限超過判別手段４から停止指令が入力されると、押し込み方向以外の力の少なくとも１つが規定値以上ある場合に、停止地点からの探索動作を行うのではなく、停止地点から記憶してある最後に押し込み方向の力が規定値以下となった地点までの予め指定されている比率に応じた地点(例えば０．５が指定されていれば、停止地点と記憶してある最後に押し込み方向の力が規定値以下となった地点の中点)まで戻る動作を行う位置指令を発生させる、戻る地点情報を含む指令発生指令を指令生成手段１に入力する。そしてロボット３が上記地点まで戻ると、探索動作を行う位置指令を発生させる指令発生指令を指令生成手段１に送信し、さらに探索動作で押し込み方向の力が規定値以下となればその地点で探索動作を停止させ、その後、元々の動作を再開する位置指令を発生させる指令発生指令(動作再開指令)を指令生成手段１に入力する。
【００５０】
この実施の形態８によれば、力制限値(力閾値)を超過した場合に、停止地点から最後に押し込み方向の力が規定値以下となった地点までの予め指定されている比率に応じた地点に戻り、そこから探索動作を行うことにより、効率よく自動的に動作を復旧継続できる効果がある。
【００５１】
実施の形態９．
この発明の実施の形態９における力制御装置の構成は基本的に図１１に示すものと同じである。実施の形態９では、復旧動作決定手段２４が、力制限超過判別手段４から停止指令が入力されると、押し込み方向以外の力の少なくとも１つが規定値以上ある場合に、停止地点で探索動作を行うのではなく、押し込み方向以外の力に関しても最後に規定値以下となった地点を記憶しておき、停止地点から記憶してある最後に押し込み方向以外の力が規定値以下となった地点までの予め指定されている比率に応じた地点(例えば０．５が指定されていれば、停止地点と記憶してある最後に押し込み方向以外の力が規定値以下となった地点の中点)まで戻る動作を行う位置指令を発生させる、戻る地点情報を含む指令発生指令を指令生成手段１に入力する。
【００５２】
そしてロボット３が上記地点まで戻ると、探索動作を行う位置指令を発生させる指令発生指令を指令生成手段１に送信し、さらに探索動作で押し込み方向の力が規定値以下となればその地点で探索動作を停止させ、その後、元々の動作を再開する位置指令を発生させる指令発生指令(動作再開指令)を指令生成手段１に入力する。
【００５３】
この実施の形態９によれば、力制限値(力閾値)を超過した場合に、停止地点から最後に押し込み方向以外の力が規定値以下となった地点までの予め指定されている比率に応じた地点に戻り、そこから探索動作を行うことにより、効率よく自動的に動作を復旧継続できる効果がある。
【００５４】
実施の形態１０．
図１２はこの発明の実施の形態１０における力制御装置の構成を示すブロック図である。実施の形態１とはパラメータ切替手段６ａ内部の作用が異なる。パラメータ切替手段６ａはロボット３から各軸モータ位置と力検出値を入力し、少なくとも１方向の力検出値が規定値(例えば記憶手段Ｍに記憶)を超えた地点を接触地点とし、接触地点からの移動量が切替地点指定手段５で指定された移動量に到達した地点で力閾値を切替えるとともに、力制御パラメータの切り替えを開始する。
【００５５】
この実施の形態１０によれば、少なくとも１方向の力検出値が規定値を超えた地点を接触地点とし、接触地点からの移動量が切替地点指定手段で指定された移動量に到達した地点で力閾値を切替え、力制御パラメータの切り替えを開始することで、より正確な力制御が行える。
【００５６】
実施の形態１１．
図１３はこの発明の実施の形態１１における力制御装置の構成を示すブロック図である。実施の形態１とは力覚センサにより力およびモーメントを検出するのではなく、力推定オブザーバ部２５を用いてロボット先端部に作用する力及びモーメントを推定し、推定した力推定値を用いる点が異なる。
【００５７】
力推定オブザーバ部２５内部では、各軸のモータ位置と各軸を駆動するモータの電流を入力する。各軸のモータ位置からモータ速度、モータ加速度を算出し、算出したモータ位置、速度、加速度からその動作で必要となる駆動トルクを算出し、モータ電流から算出される駆動トルクとの差を各軸の外乱トルクとして算出する。さらに各軸のモータ位置に基づいて算出されるヤコビ行列を用いて外乱トルクをロボット先端部に作用する力及びモーメントに変換し、変換した力及びモーメントを力推定値として力制限超過判別手段４、ロボット制御手段２へ入力する。力制限超過判別手段４、ロボット制御手段２は力推定値を力検出値の代わりに使用する。
【００５８】
この実施の形態１１によれば、力覚センサを用いることなく、動作中に指定した地点で力閾値と力制御パラメータの切替が可能となり、位置ずれ時の安全な停止と高速な挿入動作の両立が実現できる。
【００５９】
以上、この発明では、力閾値(力制限値)とコンプライアンス制御パラメータを動作中に切替えることができるため、位置ずれ発生時に過大な力の発生しやすい組み付け作業開始時は力閾値を小さく、コンプライアンス制御のパラメータを動作が柔らかくなる設定とし、反力に抗して押し付け力を発生させる必要のある挿入終了近辺区間では、力閾値を大きくし、コンプライアンス制御を動作が硬くなる設定にできるため、作業エラー発生時に過大な力が作用することを防止しながら動作を高速化できる効果がある。また、作業エラー発生時に過大な力が作用することを防止しながら、エラーの誤検知を防止できる効果もある。
【００６０】
なおこの発明は上記各実施の形態に限定されるものはなく、これらの可能な組み合わせを全て含むことは云うまでもない。
【符号の説明】
【００６１】
１ 指令生成手段、２ ロボット制御手段、３ ロボット、４ 力制限超過判別手段、５ 切替地点指定手段、６，６ａ パラメータ切替手段、７ 力覚考慮補正手段、８ 逆変換手段、９ 各軸位置制御手段、９ｎ 各軸位置制御ユニット、１０ ツール系座標変換部、１１ スティフネス行列部、１２ 可変ゲイン部、１２ａ 比例ゲイン部、１３ 直交系座標変換部、１４ 積分部、１５ 微分部、１６ ダンピング行列部、１７ ツール系座標変換部、１８ 順変換部、１９ 比例積分部、２０ 直交系座標変換部、２１ 比例部、２２ 比例積分部、２３ 微分部、２４ 復旧動作決定手段、２５ 力推定オブザーバ部、Ｇ１〜Ｇ４ 減算部、Ｉ 入力手段、Ｋ１〜Ｋ２ 加算部、Ｍ 記憶手段。
【技術分野】
【０００１】
この発明は組立、バリ取り、研磨、基板実装、検査などを行うロボット、自動組立装置、実装機、加工機、検査装置などに適用される力制御装置に関する。
【背景技術】
【０００２】
例えば下記特許文献１に記載のこの種の装置では、部品を収納したマガジンから部品を取り出し、所定位置まで移動して部品組み付け作業を行うロボットの先端部に力センサが設けられている。ロボットコントローラは、力センサで検出した力のフィードバックによるコンプライアンス制御を行い、検出した力と予め設定された力の閾値を比較して、その比較結果により次のロボット動作の決定、作業成否の判定を行っている。例えば丸棒状の部品をハンドで把持して下降して挿入を行う場合、挿入中(下降中)に検出した力が閾値を超えた場合は、挿入作業が失敗したとして下降動作を停止することにより、過大な力の発生を防止する。ハンド停止後は再び作業を行うか、あるいは、異常警報を発生する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平７−２４６６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
コネクタやゴムホースの挿入作業等のように、作業開始時は反力が小さいが作業終了時には反力が大きくなる作業が多くある。こうした作業を１つの動作で行う場合、従来の装置では、コンプライアンス制御のパラメータや力の閾値は、作業終了時に作用する大きな反力に適した値に設定する必要があった。そのため作業開始時に位置ずれがある場合でも、必要以上に大きな力の閾値を設定する必要があるため、位置ずれ時に発生する力を小さくするには作業速度を落としておく必要があり、動作時間が長くなる問題があった。
【０００５】
また検出した力が予め設定した閾値を超過するかどうかの判別は行うが、閾値を超過していない方向の力情報を活用していないため、位置ずれ時に作業エラーを発生して停止させることはできるが、その後の復旧動作については何等考慮がなされていなかった。
【０００６】
この発明は、上記の課題を解決するためになされたものであり、動作中に力閾値(力制限値)とコンプライアンス制御パラメータを切替えることで、作業を高速化しながら、位置ずれ発生時に過大な力が作用することを防止した力制御装置を提供することを得ることを目的とする。
【課題を解決するための手段】
【０００７】
この発明は、切替地点指定手段からのロボットに行わせる作業動作に従ったロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータを切り替える切替地点を指示する命令に基づいて、パラメータ切替手段が力制限値および力制御パラメータの少なくとも一方を切り替え、力制限超過判別手段が力制限値を超える力が作用していると判別した場合に、ロボットに行わせる作業動作に従ってロボットの目標位置を示す位置指令を出力する指令生成手段にロボットを減速停止させる位置指令を出力させる停止指令を入力することを特徴とする力制御装置にある。
【発明の効果】
【０００８】
この発明では、動作中に力閾値(力制限値)とコンプライアンス制御パラメータを切替えることで、作業を高速化しながら、位置ずれ発生時に過大な力が作用することを防止することができる。
【図面の簡単な説明】
【０００９】
【図１】この発明の実施の形態１、２における力制御装置の構成を示すブロック図である。
【図２】図１のロボット制御手段の内部構成の一例を示す図である。
【図３】図２の各軸位置制御手段の内部構成の一例を示す図である。
【図４】図２の力覚考慮補正手段の内部構成の一例を示す図である。
【図５】図２の力覚考慮補正手段の内部構成の別の例を示す図である。
【図６】図２の力覚考慮補正手段の内部構成のさらに別の例を示す図である。
【図７】図２の力覚考慮補正手段の内部構成のさらに別の例を示す図である。
【図８】この発明の実施の形態３における力制御装置の構成を示すブロック図である。
【図９】図８の力覚考慮補正手段の内部構成の一例を示す図である。
【図１０】この発明の実施の形態４における力制御装置の構成を示すブロック図である。
【図１１】この発明の実施の形態５〜９における力制御装置の構成を示すブロック図である。
【図１２】この発明の実施の形態１０における力制御装置の構成を示すブロック図である。
【図１３】この発明の実施の形態１１における力制御装置の構成を示すブロック図である。
【発明を実施するための形態】
【００１０】
以下、この発明による力制御装置を各実施の形態に従って図面を用いて説明する。なお、各実施の形態において、同一もしくは相当部分は同一符号で示し、重複する説明は省略する。
【００１１】
実施の形態１．
図１はこの発明の実施の形態１における力制御装置の構成を示すブロック図である。指令生成手段１は例えば記憶手段Ｍに予め格納されているロボット３に行わせる作業の作業動作プログラムに基づいてロボット制御手段２にロボット３の時々刻々の目標位置を示す位置指令を入力する。ロボット制御手段２は与えられた位置指令および後述する例えばコンプライアンス制御の力制御パラメータ、ロボット３からのフィードバック信号に従ってロボット３を追従制御する。
【００１２】
ロボット制御手段２により制御されるロボット３は、１軸もしくは複数軸のモータ(図示省略)を備えるものであり、その一例として産業用ロボットと呼ばれる６軸垂直多関節ロボット、４軸水平多関節ロボット等がある。ロボット３の先端部近辺もしくはロボットが作業する対象には作用する力とモーメントを計測するための力覚センサが取り付けられており(共に図示省略)、力覚センサで測定された力及びモーメントは力検出値としてロボット制御手段２および力制限超過判別手段４に入力される。
【００１３】
力検出値は６軸の力覚センサを用いている場合は３軸方向の力と各軸周りのモーメントであり、５軸以下の力覚センサを用いている場合は、３軸方向の力と各軸周りのモーメントのうち、計測できるモーメントであり、各力覚センサがこれらのデータをロボット制御手段２及び力制限超過判別手段４に送信する。またロボット３を駆動する各軸のモータにはエンコーダやレゾルバなどの各軸のモータ位置を測定するセンサが取り付けられており(図示省略)、こうしたセンサで測定した各軸のモータ位置はロボット制御手段２に送信される。
【００１４】
力制限超過判別手段４では、パラメータ切替手段６から入力される力閾値(力制限値)とロボット３の力覚センサから送信される力検出値を比較し、少なくとも１つの軸方向の力もしくはモーメントが力閾値を超えた場合に、指令生成手段１に停止指令を入力する。この実施の形態では力制限超過の判別を検出した力とモーメントの各成分に対して実施するが、合成した力やモーメントに対して実施してもよい。指令生成手段１は力制限超過判別手段４から停止指令を受け取ると、現在の位置・速度から減速停止する指令を生成してロボット制御手段２に入力する。
【００１５】
切替地点指定手段５は、上記作業動作プログラムの動作に従ってパラメータ切替手段６でパラメータを切替える地点と切替後のパラメータを順次指定する。パラメータ切替手段６で切替えるパラメータが力閾値と力制御パラメータの２種類の場合、例えばロボット３の動作を記述するロボットプログラム内で下記のような命令を実行して指定する。
【００１６】
FCNT ３０，２
【００１７】
ここで、「FCNT」は切替地点を指定するためのロボット言語(命令)で、「３０」は動作開始から３０％の地点でパラメータの切替を開始することを指定し、「２」は切替後のパラメータ条件番号である。条件番号ごとに力閾値と力制御パラメータの組合せが例えば記憶手段Ｍに記録されており、パラメータ切替手段６は位置指令が切替地点指定手段５で設定された地点に到達した段階で、指定された条件番号に対応する力閾値と力制御パラメータを読み出す。
【００１８】
なお図１において、ロボット３を除く指令生成手段１、ロボット制御手段２、力制限超過判別手段４、切替地点指定手段５、パラメータ切替手段６は例えば１つのコンピュータ(ロボットコントローラ)により構成され、記憶手段Ｍは各手段で共有する該コンピュータ内のメモリ(ＲＯＭ、ＲＡＭ等を含むものとする)を示す。
【００１９】
パラメータ切替手段６では、力閾値に関しては、指定された切替地点通過後に直ちに切替える。力制御パラメータに関しては、切替前の力制御パラメータから切替後の力制御パラメータへ例えば２段の移動平均フィルタを介した出力で切替えを行う。切替の開始を指定された切替地点を通過時点とする。力制御パラメータの切替は１段もしくは３段以上の移動平均フィルタを用いてもよいし、移動平均以外の、例えば１次遅れフィルタなどを用いてもよい。また、切替前の力制御パラメータから切替後の力制御パラメータまで切替える関数を内部に備えておき、備えてある関数を用いて徐々に切替えてもよい。また、この実施の形態では動作開始地点からの移動距離の割合で切替開始地点を指定したが、動作終了地点までの移動距離の割合、動作開始地点からの移動距離、動作終了地点までの移動距離、動作開始からの経過時間などで指定してもよい。
【００２０】
ロボット制御手段２の内部構成の一例を図２に示す。指令生成手段１で生成した位置指令を力覚考慮補正手段７の出力を加算部Ｋ１で加算することで補正する。逆変換手段８は、手先位置姿勢を実現する各軸のモータ位置を算出する逆変換計算を行い、力覚考慮補正手段７の出力を加算することで補正した位置指令をロボットの各軸の位置指令である関節位置指令に変換する。変換した関節位置指令は各軸位置制御手段９に入力される。各軸位置制御手段９は入力された関節位置指令に各軸のモータ位置が追従するように制御を行う。
【００２１】
各軸位置制御手段９の内部構成の一例を図３に示す。図３の各軸位置制御手段９のモータ毎に設けられた各軸位置制御ユニット９ｎでは、各軸の(関節)位置指令と当該軸のモータ位置の差を減算部Ｇ１で求めて比例部２１に入力する。そして比例部２１の出力と微分部２３で求めたモータ位置の微分値から減算部Ｇ２で算出したモータ速度の差を求め、求めた差を比例積分部２２に入力する。上記制御系は位置比例制御系の内部に速度比例積分制御系がある構成になっており、モータの位置制御系としてはよく使われる構成である。
【００２２】
なお、この実施の形態では各軸位置制御ユニット９ｎの構成として、各軸毎にモータ位置制御と速度制御を行うフィードバック制御系の構成としたが、各軸ごとにオブザーバを含む制御系であってもよいし、フィードフォワードを含む制御系であってもよい。また、全て各軸毎に独立した構成ではなく、軸間の干渉トルクをフィードフォワードもしくはフィードバックで補正する構成であってもよい。
【００２３】
次に、力覚考慮補正手段７の内部構成の一例を図４に示す。力覚考慮補正手段７の内部では、まず順変換部１８において各軸モータ位置からそのときの手先位置姿勢を算出する順変換を行い手先位置姿勢を算出し、指令生成手段１で生成した位置指令(手先位置姿勢の指令)と順変換部１８で算出した手先位置姿勢の差を減算部Ｇ３で算出し、算出した差をツール座標系にツール系座標変換部１０により変換する。ツール座標系とは、ロボット先端に取り付けたツールやハンドに固定された座標系である。
【００２４】
ツール系座標変換部１０の出力をスティフネス行列部１１に入力し、スティフネス行列とツール系座標変換部１０の出力であるベクトルの積を求め、その積をスティフネス行列部１１の出力とする。さらにツール系座標変換部１０の出力の各要素の微分値を微分部１５で算出し、算出した微分値から構成されるベクトルとダンピング行列の積をダンピング行列部１６の出力とし、スティフネス行列部１１とダンピング行列部１６の出力を加算部Ｋ２で加算する。
【００２５】
次に、ロボット３の力覚センサで検出した力検出値をツール系座標変換部１７でツール座標系に変換し、変換した力検出値を先ほどの加算部Ｋ２の出力結果から減算部Ｇ４で減算する。減算した結果の各要素にパラメータ切替手段６で切替えられる可変の比例ゲイン値を可変ゲイン部１２で乗じ、その後、直交系座標変換部１３で直交座標系への座標変換を行い、積分部１４で積分を行ってから位置指令修正量(位置指令補正量)としてロボット制御手段２の加算部Ｋ１に入力する。可変ゲイン部１２にはパラメータ切替手段６で切替える力制御パラメータが使用される。
【００２６】
図４では、力制御系のパラメータとして力検出値との差に対する比例ゲインのみを変更する場合を例に挙げて説明したが、力覚考慮補正手段７は図５に示すように、スティフネス行列部１１及びダンピング行列部１６にそれぞれパラメータ切替手段６で切替える力制御パラメータを使用して各行列要素値を与え、スティフネス行列及びダンピング行列の各要素も指定された切替地点から切替えるようにしてもよい。
【００２７】
また、図６に示すように、スティフネス行列部１１及びダンピング行列部１６にそれぞれパラメータ切替手段６で切替える力制御パラメータを使用して各行列要素値を与え、スティフネス行列及びダンピング行列の各要素を指定された切替地点から切替、可変ゲイン部１２と積分部１４の代わりに比例積分部１９を設けて力検出値との差に乗じるゲインは切替えない構成にしてもよい。
【００２８】
また、ツール座標系にいったん変換してから位置指令修正量(位置指令補正量)を算出するのではなく、図７に示すように、ツール系座標変換部１０及び直交系座標変換部１３を設けず、ツール系座標変換部１７の代わりに直交系座標変換部２０を設けることで、直交座標系で位置指令修正量(位置指令補正量)を算出して補正してもよい。
【００２９】
またこの実施の形態では動作中の１箇所で力閾値及び力制御パラメータを切替える場合を例に挙げて説明したが、例えば
【００３０】
FCNT ３０，２，５０，３
【００３１】
と記載し、移動距離の３０％を越えた地点で条件番号２へ切替、移動距離の５０％を越えた地点で条件番号３へ切替えるなど、動作中の２箇所以上で切替える構成にしてもよい。また、パラメータ番号ではなく、切替えるパラメータの値を直接記載してもよい。
【００３２】
この実施の形態１によれば、動作中の切替地点をロボットを動作させるロボットプログラム中の指令で指定された地点で力閾値と力制御パラメータを切替えることができるため、動作中に指定した地点で力閾値と力制御パラメータの切替が可能となり、位置ずれ時の安全な停止と高速な挿入動作の両立が実現できる。
また、動作途中で力制御系のパラメータを変更するため、接触直後は柔らかくロボットを動作させ、押し込みや挿入の最終段階ではロボットを固く動作させることも可能となり、接触開始直後は柔らかくしてダメージを抑えながら、反力に対抗して押し込み力が必要とうなる作業を高速に実施することが可能となる。
また、プログラムの指令で動作直前に切替地点を指定できるため、動作ごとにきめ細かく設定することができる。
【００３３】
実施の形態２．
以下にこの発明の実施の形態２における力制御装置を説明する。装置の構成は基本的には図１に示すものと同じであるが、切替地点指定手段５の作用が異なる。例えば切替地点指定手段５からパラメータ切替手段６に送られる上述の命令のパラメータ(切替地点や条件番号)を、コンピュータに接続された手動操作盤またはパソコンからなる図１に破線で示す入力手段Ｉから値を書き換えるようにして指定する。
【００３４】
例えば上述のFCNTのパラメータ(ファクタ)の値をFCNT(３０，１，２)と設定すると、パラメータ切替手段６は動作開始からその動作の移動量の３０％を超過するまでは条件番号１で設定されている力閾値と力制御パラメータを力制限超過判別手段４及びロボット制御手段２に入力し、動作開始からその動作の移動量の３０％を超過後は、力閾値として条件番号２で設定されている値を力制限超過判別手段４に入力するとともに、ロボット制御手段２へ入力する力制御パラメータの値を徐々に条件番号２の値へ切替える。
【００３５】
この実施の形態２によれば、上記に加え、入力手段Ｉからの入力により動作直前に命令のパラメータを指定できるため、動作ごとにきめ細かく設定することができる。
【００３６】
実施の形態３．
図８はこの発明の実施の形態３における力制御装置の構成を示すブロック図である。実施の形態１とは異なり、パラメータ切替手段６では力閾値のみ切替える。したがって実施の形態１の図２に示されているロボット制御手段２の力覚考慮補正手段７の内部も、実施の形態３では図９に示すように可変ゲイン部１２の代わりに比例ゲイン部１２ａが設けられた固定パラメータの制御系となる。
【００３７】
この実施の形態３によれば、動作中の切替地点をロボットを動作させる指令ロボットプログラムの指令で指定された地点で力閾値を切替えることができるため、動作中に指定した地点で力閾値の切替が可能となり、位置ずれ時の安全な停止と高速な挿入動作の両立が実現できる。
【００３８】
実施の形態４．
図１０はこの発明の実施の形態４における力制御装置の構成を示すブロック図である。実施の形態１とは異なり、パラメータ切替手段６では力制御パラメータのみ切替える。
【００３９】
この実施の形態４によれば、動作途中で力制御系のパラメータを変更するため、接触直後は柔らかくロボットを動作させ、押し込みや挿入の最終段階ではロボットを固く動作させることも可能となり、接触開始直後は柔らかくしてダメージを抑えながら、反力に対抗して押し込み力が必要となる作業を高速に実施することが可能となる。
【００４０】
実施の形態５．
図１１はこの発明の実施の形態５における力制御装置の構成を示すブロック図である。実施の形態１では力制限値(力閾値)超過後、停止指令を指令生成手段１に入力していたが、実施の形態５では指令生成手段１に加えて復旧動作決定手段２４を設け、復旧動作決定手段２４にも停止指令を入力する。なおこの実施の形態では、図１１において、ロボット３の力検出値、各軸モータ位置のデータは復旧動作決定手段２４へは送られない。
【００４１】
復旧動作決定手段２４は、停止指令が入力されると、その時実行中の作業動作の動作開始地点までの退避動作を行わせるための指令生成命令を指令生成手段１に入力する。指令生成手段１は指令生成命令を受けると、作業動作プログラムより該動作の動作開始地点を求め、上記退避動作のための位置指令をロボット制御手段２へ入力する。
【００４２】
この実施の形態５によれば、力制限値(力閾値)を超過した場合に、ロボットの状態が自動的に、直前の動作開始地点まで戻ることにより、力制限値を超過した状態が継続することを防止できる効果がある。
【００４３】
実施の形態６．
この発明の実施の形態６における力制御装置の構成は基本的に図１１に示すものと同じである。復旧動作決定手段２４では、ロボット３から時々刻々の各軸モータ位置と力検出値を入力し、動作中に力検出値が最後に所定の規定値以下となった地点を各軸モータ位置として記憶しておく(例えば記憶手段Ｍに記憶)。そして力制限超過判別手段４から停止指令が入力されると、記憶してある最後に規定値以下となった地点まで戻る退避動作を行わせるための、規定値以下となった地点の各軸モータ位置を含む指令生成命令を指令生成手段１に入力する。指令生成手段１は指令生成命令を受けると、受け取った規定値以下となった地点の各軸モータ位置に従って、上記退避動作のための位置指令を、ロボット制御手段２へ入力する。
【００４４】
規定値は値そのものを予め設定(例えば記憶手段Ｍに記憶)しておいてよいし、力閾値との比率(例えば０．８倍)を設定し、常に力制限超過判別手段４で使用する力閾値(力制限値)に設定された比率(例えば記憶手段Ｍに記憶またはプログラム中に計算式として入れる)を乗じた値を規定値としてもよい。その場合、パラメータ切替手段６は図１１に破線で示すように、常に力閾値を復旧動作決定手段２４にも入力する。また、この実施の形態では力閾値を復旧動作決定手段２４が記憶してある各軸モータ位置に基づいて退避動作を行わせるが、モータ位置を記憶するのではなく、動作中に力検出値が最後に規定値以下となった時の指令生成手段１の出力する位置指令を記憶しておき、記憶してある位置指令の地点まで戻る動作を行わせてもよい。その場合、指令生成手段１は図１１に破線で示すように、常に位置指令を復旧動作決定手段２４にも入力する。復旧動作決定手段２４は規定値以下となった地点の位置指令を含む指令生成命令を指令生成手段１に入力する。
【００４５】
この実施の形態６によれば、力制限値(力閾値)を超過した場合に、ロボットの状態が自動的に、力が規定値以下となる地点まで戻ることにより、力制限を超過した状態が継続することを防止できる効果がある。また、動作開始地点までの退避動作に比べて、戻るときの移動量を少なくできるため、退避動作の時間短縮、また予想外の衝突などを防止できる効果もある
【００４６】
実施の形態７．
この発明の実施の形態７における力制御装置の構成は基本的に図１１に示すものと同じである。実施の形態７では、復旧動作決定手段２４が、停止指令が入力されたときの力検出値により、その後の動作を選択する。押し込み方向以外の力検出値が全て規定値以下の場合は、実施の形態６と同様に押し込み方向の力検出値が最後に規定値以下となった地点まで戻る退避動作を行わせる指令生成指令を指令生成手段１に入力する。
【００４７】
押し込み方向以外の力の少なくとも１つが規定値以上の場合は、停止した地点で探索動作を行わせる指令発生指令を指令生成手段１に入力し、ロボット３の動作を戻す方向に探索動作を行う退避動作を行わせる。探索動作の途中で押し込み方向の力が規定値以下となればその地点で停止させ、その後、元々の動作を再開する位置指令を発生させる指令発生指令(動作再開指令)を指令生成手段１に入力する。
【００４８】
この実施の形態７によれば、力制限値(力閾値)を超過した場合に、探索動作を行うことにより自動的に動作を復旧継続できる効果がある。
【００４９】
実施の形態８．
この発明の実施の形態８における力制御装置の構成は基本的に図１１に示すものと同じである。実施の形態８では、復旧動作決定手段２４は、力制限超過判別手段４から停止指令が入力されると、押し込み方向以外の力の少なくとも１つが規定値以上ある場合に、停止地点からの探索動作を行うのではなく、停止地点から記憶してある最後に押し込み方向の力が規定値以下となった地点までの予め指定されている比率に応じた地点(例えば０．５が指定されていれば、停止地点と記憶してある最後に押し込み方向の力が規定値以下となった地点の中点)まで戻る動作を行う位置指令を発生させる、戻る地点情報を含む指令発生指令を指令生成手段１に入力する。そしてロボット３が上記地点まで戻ると、探索動作を行う位置指令を発生させる指令発生指令を指令生成手段１に送信し、さらに探索動作で押し込み方向の力が規定値以下となればその地点で探索動作を停止させ、その後、元々の動作を再開する位置指令を発生させる指令発生指令(動作再開指令)を指令生成手段１に入力する。
【００５０】
この実施の形態８によれば、力制限値(力閾値)を超過した場合に、停止地点から最後に押し込み方向の力が規定値以下となった地点までの予め指定されている比率に応じた地点に戻り、そこから探索動作を行うことにより、効率よく自動的に動作を復旧継続できる効果がある。
【００５１】
実施の形態９．
この発明の実施の形態９における力制御装置の構成は基本的に図１１に示すものと同じである。実施の形態９では、復旧動作決定手段２４が、力制限超過判別手段４から停止指令が入力されると、押し込み方向以外の力の少なくとも１つが規定値以上ある場合に、停止地点で探索動作を行うのではなく、押し込み方向以外の力に関しても最後に規定値以下となった地点を記憶しておき、停止地点から記憶してある最後に押し込み方向以外の力が規定値以下となった地点までの予め指定されている比率に応じた地点(例えば０．５が指定されていれば、停止地点と記憶してある最後に押し込み方向以外の力が規定値以下となった地点の中点)まで戻る動作を行う位置指令を発生させる、戻る地点情報を含む指令発生指令を指令生成手段１に入力する。
【００５２】
そしてロボット３が上記地点まで戻ると、探索動作を行う位置指令を発生させる指令発生指令を指令生成手段１に送信し、さらに探索動作で押し込み方向の力が規定値以下となればその地点で探索動作を停止させ、その後、元々の動作を再開する位置指令を発生させる指令発生指令(動作再開指令)を指令生成手段１に入力する。
【００５３】
この実施の形態９によれば、力制限値(力閾値)を超過した場合に、停止地点から最後に押し込み方向以外の力が規定値以下となった地点までの予め指定されている比率に応じた地点に戻り、そこから探索動作を行うことにより、効率よく自動的に動作を復旧継続できる効果がある。
【００５４】
実施の形態１０．
図１２はこの発明の実施の形態１０における力制御装置の構成を示すブロック図である。実施の形態１とはパラメータ切替手段６ａ内部の作用が異なる。パラメータ切替手段６ａはロボット３から各軸モータ位置と力検出値を入力し、少なくとも１方向の力検出値が規定値(例えば記憶手段Ｍに記憶)を超えた地点を接触地点とし、接触地点からの移動量が切替地点指定手段５で指定された移動量に到達した地点で力閾値を切替えるとともに、力制御パラメータの切り替えを開始する。
【００５５】
この実施の形態１０によれば、少なくとも１方向の力検出値が規定値を超えた地点を接触地点とし、接触地点からの移動量が切替地点指定手段で指定された移動量に到達した地点で力閾値を切替え、力制御パラメータの切り替えを開始することで、より正確な力制御が行える。
【００５６】
実施の形態１１．
図１３はこの発明の実施の形態１１における力制御装置の構成を示すブロック図である。実施の形態１とは力覚センサにより力およびモーメントを検出するのではなく、力推定オブザーバ部２５を用いてロボット先端部に作用する力及びモーメントを推定し、推定した力推定値を用いる点が異なる。
【００５７】
力推定オブザーバ部２５内部では、各軸のモータ位置と各軸を駆動するモータの電流を入力する。各軸のモータ位置からモータ速度、モータ加速度を算出し、算出したモータ位置、速度、加速度からその動作で必要となる駆動トルクを算出し、モータ電流から算出される駆動トルクとの差を各軸の外乱トルクとして算出する。さらに各軸のモータ位置に基づいて算出されるヤコビ行列を用いて外乱トルクをロボット先端部に作用する力及びモーメントに変換し、変換した力及びモーメントを力推定値として力制限超過判別手段４、ロボット制御手段２へ入力する。力制限超過判別手段４、ロボット制御手段２は力推定値を力検出値の代わりに使用する。
【００５８】
この実施の形態１１によれば、力覚センサを用いることなく、動作中に指定した地点で力閾値と力制御パラメータの切替が可能となり、位置ずれ時の安全な停止と高速な挿入動作の両立が実現できる。
【００５９】
以上、この発明では、力閾値(力制限値)とコンプライアンス制御パラメータを動作中に切替えることができるため、位置ずれ発生時に過大な力の発生しやすい組み付け作業開始時は力閾値を小さく、コンプライアンス制御のパラメータを動作が柔らかくなる設定とし、反力に抗して押し付け力を発生させる必要のある挿入終了近辺区間では、力閾値を大きくし、コンプライアンス制御を動作が硬くなる設定にできるため、作業エラー発生時に過大な力が作用することを防止しながら動作を高速化できる効果がある。また、作業エラー発生時に過大な力が作用することを防止しながら、エラーの誤検知を防止できる効果もある。
【００６０】
なおこの発明は上記各実施の形態に限定されるものはなく、これらの可能な組み合わせを全て含むことは云うまでもない。
【符号の説明】
【００６１】
１ 指令生成手段、２ ロボット制御手段、３ ロボット、４ 力制限超過判別手段、５ 切替地点指定手段、６，６ａ パラメータ切替手段、７ 力覚考慮補正手段、８ 逆変換手段、９ 各軸位置制御手段、９ｎ 各軸位置制御ユニット、１０ ツール系座標変換部、１１ スティフネス行列部、１２ 可変ゲイン部、１２ａ 比例ゲイン部、１３ 直交系座標変換部、１４ 積分部、１５ 微分部、１６ ダンピング行列部、１７ ツール系座標変換部、１８ 順変換部、１９ 比例積分部、２０ 直交系座標変換部、２１ 比例部、２２ 比例積分部、２３ 微分部、２４ 復旧動作決定手段、２５ 力推定オブザーバ部、Ｇ１〜Ｇ４ 減算部、Ｉ 入力手段、Ｋ１〜Ｋ２ 加算部、Ｍ 記憶手段。
【特許請求の範囲】
【請求項１】
切替地点指定手段からのロボットに行わせる作業動作に従ったロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータを切り替える切替地点を指示する命令に基づいて、パラメータ切替手段が力制限値および力制御パラメータの少なくとも一方を切り替え、力制限超過判別手段が力制限値を超える力が作用していると判別した場合に、ロボットに行わせる作業動作に従ってロボットの目標位置を示す位置指令を出力する指令生成手段にロボットを減速停止させる位置指令を出力させる停止指令を入力することを特徴とする力制御装置。
【請求項２】
モータを設けたロボットに行わせる作業動作に従ったロボットの時々刻々の目標位置を示す位置指令を出力する上記指令生成手段と、
動作中にロボットに行わせる作業動作に従ってロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータ条件、および上記パラメータ条件に切り替える切替地点を指示する命令を発生する上記切替地点指定手段と、
上記切替地点指定手段からの命令に基づき指定された切替地点で上記力制限値および力制御パラメータの少なくとも一方の切替を開始する上記パラメータ切替手段と、
上記指令生成手段からの位置指令と上記パラメータ切替手段からの力制御パラメータとロボットからの力検出値およびモータ位置を示す信号に従ってロボットを制御するロボット制御手段と、
ロボットからの力検出値が上記パラメータ切替手段からの力制限値を超えた場合に停止指令を出力する上記力制限超過判別手段と、
を備え、
上記指令生成手段が上記力制限超過判別手段から停止指令を受けるとロボットを減速停止させる位置指令を出力する、
ことを特徴とする請求項１に記載の力制御装置。
【請求項３】
上記切替地点指定手段において上記パラメータ条件および切替地点の少なくとも一方を書き変える入力手段をさらに備えたことを特徴とする請求項１または２に記載の力制御装置。
【請求項４】
上記力制限超過判別手段から停止指令を受けた時に、現在実行中の動作の動作開始地点までの退避動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力する復旧動作決定手段を備えることを特徴とする請求項２または３に記載の力制御装置。
【請求項５】
ロボットからの上記力検出値およびモータ位置を入力すると共に力検出値が最後に規定値以下になった時のモータ位置を記憶し、上記力制限超過判別手段から停止指令を受けた時に、記憶してある力検出値が最後に規定値以下となった地点まで戻る退避動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力する復旧動作決定手段を備えることを特徴とする請求項２または３に記載の力制御装置。
【請求項６】
ロボットに行わせる作業動作が押し込みであり、ロボットが複数のモータを設けロボットからそれぞれ複数の力検出値およびモータ位置を入力すると共に少なくとも押し込み方向の力検出値が最後に規定値以下になった時のモータ位置を記憶し、
押し込み方向の力検出値が上記力制限値を超えて上記力制限超過判別手段から停止指令を受けた時に、押し込み方向以外の方向の力検出値の規定値との関係に従ってそれぞれ定められた地点からの探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力する復旧動作決定手段を備えることを特徴とする請求項２または３に記載の力制御装置。
【請求項７】
上記復旧動作決定手段が、
上記力制限超過判別手段からの停止指令を受けた時に、
押し込み方向以外の方向の力検出値が全て規定値以下の場合には、記憶してある押し込み方向の力検出値が最後に規定値以下となった地点まで戻る退避動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力し、
押し込み方向以外の方向の少なくとも１つの力検出値が規定値以上の場合には、停止地点から探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項６に記載の力制御装置。
【請求項８】
上記復旧動作決定手段が、
上記力制限超過判別手段からの停止指令を受けた時に、
押し込み方向以外の方向の少なくとも１つの力検出値が規定値以上の場合に、停止地点から記憶してある押し込み方向の力検出値が最後に規定値以下となった地点までの所定比率の地点まで戻り、その後、探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項６に記載の力制御装置。
【請求項９】
上記復旧動作決定手段が、
押し込み方向以外の方向の力検出値が最後に規定値以下になった時のモータ位置も記憶し、上記力制限超過判別手段からの停止指令を受けた時に、
押し込み方向以外の方向の少なくとも１つの力検出値が規定値以上の場合に、停止地点から記憶してある最後に押し込み方向以外の方向の力検出値が規定値以下となった地点までの所定比率の地点まで戻り、その後、探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項６に記載の力制御装置。
【請求項１０】
上記復旧動作決定手段が、上記探索動作において、押し込み方向の力検出値が規定値以下になった地点で元の動作を再開させるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項７から９までのいずれか１項に記載の力制御装置。
【請求項１１】
上記復旧動作決定手段が、上記指令生成手段からの位置指令を入力すると共に力検出値が最後に規定値以下になった時の位置指令を記憶して上記モータ位置の代わりに使用することを特徴とする請求項５から１０までのいずれか１項に記載の力制御装置。
【請求項１２】
上記復旧動作決定手段が、上記力検出値の規定値を上記パラメータ切替手段の力制限値に従って設定することを特徴とする請求項５から１１までのいずれか１項に記載の力制御装置。
【請求項１３】
上記パラメータ切替手段が、ロボットからモータ位置と力検出値を入力し、少なくとも１方向の力検出値が規定値を超えた地点を接触地点とし、接触地点からの移動量が上記切替地点指定手段で指定された切替地点に到達した地点で力制限値を切替えるとともに、力制御パラメータの切り替えを開始することを特徴とする請求項２に記載の力制御装置。
【請求項１４】
ロボットのモータのモータ位置とモータ電流を入力し、ロボット先端部に作用する力及びモーメントを推定し力推定値として求める力推定オブザーバ部を備え、推定した力推定値を力検出値の代わりに使用することを特徴とする請求項２に記載の力制御装置。
【請求項１】
切替地点指定手段からのロボットに行わせる作業動作に従ったロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータを切り替える切替地点を指示する命令に基づいて、パラメータ切替手段が力制限値および力制御パラメータの少なくとも一方を切り替え、力制限超過判別手段が力制限値を超える力が作用していると判別した場合に、ロボットに行わせる作業動作に従ってロボットの目標位置を示す位置指令を出力する指令生成手段にロボットを減速停止させる位置指令を出力させる停止指令を入力することを特徴とする力制御装置。
【請求項２】
モータを設けたロボットに行わせる作業動作に従ったロボットの時々刻々の目標位置を示す位置指令を出力する上記指令生成手段と、
動作中にロボットに行わせる作業動作に従ってロボット先端に作用する力の力制限値およびコンプライアンス制御の力制御パラメータの少なくとも一方を含むパラメータ条件、および上記パラメータ条件に切り替える切替地点を指示する命令を発生する上記切替地点指定手段と、
上記切替地点指定手段からの命令に基づき指定された切替地点で上記力制限値および力制御パラメータの少なくとも一方の切替を開始する上記パラメータ切替手段と、
上記指令生成手段からの位置指令と上記パラメータ切替手段からの力制御パラメータとロボットからの力検出値およびモータ位置を示す信号に従ってロボットを制御するロボット制御手段と、
ロボットからの力検出値が上記パラメータ切替手段からの力制限値を超えた場合に停止指令を出力する上記力制限超過判別手段と、
を備え、
上記指令生成手段が上記力制限超過判別手段から停止指令を受けるとロボットを減速停止させる位置指令を出力する、
ことを特徴とする請求項１に記載の力制御装置。
【請求項３】
上記切替地点指定手段において上記パラメータ条件および切替地点の少なくとも一方を書き変える入力手段をさらに備えたことを特徴とする請求項１または２に記載の力制御装置。
【請求項４】
上記力制限超過判別手段から停止指令を受けた時に、現在実行中の動作の動作開始地点までの退避動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力する復旧動作決定手段を備えることを特徴とする請求項２または３に記載の力制御装置。
【請求項５】
ロボットからの上記力検出値およびモータ位置を入力すると共に力検出値が最後に規定値以下になった時のモータ位置を記憶し、上記力制限超過判別手段から停止指令を受けた時に、記憶してある力検出値が最後に規定値以下となった地点まで戻る退避動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力する復旧動作決定手段を備えることを特徴とする請求項２または３に記載の力制御装置。
【請求項６】
ロボットに行わせる作業動作が押し込みであり、ロボットが複数のモータを設けロボットからそれぞれ複数の力検出値およびモータ位置を入力すると共に少なくとも押し込み方向の力検出値が最後に規定値以下になった時のモータ位置を記憶し、
押し込み方向の力検出値が上記力制限値を超えて上記力制限超過判別手段から停止指令を受けた時に、押し込み方向以外の方向の力検出値の規定値との関係に従ってそれぞれ定められた地点からの探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力する復旧動作決定手段を備えることを特徴とする請求項２または３に記載の力制御装置。
【請求項７】
上記復旧動作決定手段が、
上記力制限超過判別手段からの停止指令を受けた時に、
押し込み方向以外の方向の力検出値が全て規定値以下の場合には、記憶してある押し込み方向の力検出値が最後に規定値以下となった地点まで戻る退避動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力し、
押し込み方向以外の方向の少なくとも１つの力検出値が規定値以上の場合には、停止地点から探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項６に記載の力制御装置。
【請求項８】
上記復旧動作決定手段が、
上記力制限超過判別手段からの停止指令を受けた時に、
押し込み方向以外の方向の少なくとも１つの力検出値が規定値以上の場合に、停止地点から記憶してある押し込み方向の力検出値が最後に規定値以下となった地点までの所定比率の地点まで戻り、その後、探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項６に記載の力制御装置。
【請求項９】
上記復旧動作決定手段が、
押し込み方向以外の方向の力検出値が最後に規定値以下になった時のモータ位置も記憶し、上記力制限超過判別手段からの停止指令を受けた時に、
押し込み方向以外の方向の少なくとも１つの力検出値が規定値以上の場合に、停止地点から記憶してある最後に押し込み方向以外の方向の力検出値が規定値以下となった地点までの所定比率の地点まで戻り、その後、探索動作を行わせるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項６に記載の力制御装置。
【請求項１０】
上記復旧動作決定手段が、上記探索動作において、押し込み方向の力検出値が規定値以下になった地点で元の動作を再開させるための位置指令を出力させる指令生成命令を上記指令生成手段に入力することを特徴とする請求項７から９までのいずれか１項に記載の力制御装置。
【請求項１１】
上記復旧動作決定手段が、上記指令生成手段からの位置指令を入力すると共に力検出値が最後に規定値以下になった時の位置指令を記憶して上記モータ位置の代わりに使用することを特徴とする請求項５から１０までのいずれか１項に記載の力制御装置。
【請求項１２】
上記復旧動作決定手段が、上記力検出値の規定値を上記パラメータ切替手段の力制限値に従って設定することを特徴とする請求項５から１１までのいずれか１項に記載の力制御装置。
【請求項１３】
上記パラメータ切替手段が、ロボットからモータ位置と力検出値を入力し、少なくとも１方向の力検出値が規定値を超えた地点を接触地点とし、接触地点からの移動量が上記切替地点指定手段で指定された切替地点に到達した地点で力制限値を切替えるとともに、力制御パラメータの切り替えを開始することを特徴とする請求項２に記載の力制御装置。
【請求項１４】
ロボットのモータのモータ位置とモータ電流を入力し、ロボット先端部に作用する力及びモーメントを推定し力推定値として求める力推定オブザーバ部を備え、推定した力推定値を力検出値の代わりに使用することを特徴とする請求項２に記載の力制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２０１１−１０４７４０（Ｐ２０１１−１０４７４０Ａ）
【公開日】平成２３年６月２日（２０１１．６．２）
【国際特許分類】
【出願番号】特願２００９−２６３９７５（Ｐ２００９−２６３９７５）
【出願日】平成２１年１１月１９日（２００９．１１．１９）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２１年度、独立行政法人新エネルギー・産業技術総合開発機構、「戦略的先端ロボット要素技術開発プロジェクト、柔軟物も取扱える生産用ロボットシステム（次世代産業用ロボット分野）、ＦＡ機器組立ロボットシステムの研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２３年６月２日（２０１１．６．２）
【国際特許分類】
【出願日】平成２１年１１月１９日（２００９．１１．１９）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２１年度、独立行政法人新エネルギー・産業技術総合開発機構、「戦略的先端ロボット要素技術開発プロジェクト、柔軟物も取扱える生産用ロボットシステム（次世代産業用ロボット分野）、ＦＡ機器組立ロボットシステムの研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]