JP2008307634A

JP2008307634A - 嵌合装置

Info

Publication number: JP2008307634A
Application number: JP2007157333A
Authority: JP
Inventors: Takayuki Sato; 貴之佐藤; Nobuaki Yamaoka; 宣章山岡
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 2007-06-14
Filing date: 2007-06-14
Publication date: 2008-12-25
Anticipated expiration: 2027-06-14
Also published as: US8369983B2; JP4271249B2; CN101323111A; CN101323111B; EP2002945A1; US20080312769A1; EP2002945B1

Abstract

【課題】コンプライアンス制御を用いた嵌合装置において、従来よりも高速で安定した嵌合を行うこと可能にする。
【解決手段】嵌合装置１０は、ワークを把持するハンドを先端に備えたロボットアーム１２と、ハンドに把持されたワークが受ける力及びモーメントを検出するための力検出器１４と、ロボットアーム１２の動作を制御するための制御装置１６とを備える。制御装置１６は、ワークを嵌合させるように動作指令を作成する動作指令作成部３０と、ワークの接触中に力検出器１４によって検出される力及びモーメントの最大値又はワークが初めて接触したときに力検出器１４によって検出された力及びモーメントの何れかに基づき、検出される力及びモーメントが閾値以下になるまで、嵌合方向に対して垂直な方向に関するハンドの位置と嵌合方向に対して垂直な軸線周りのハンドの姿勢とを修正するように動作指令を補正する動作指令補正部３２とを備える。
【選択図】図２

Description

本発明は、ロボットを用いてコンプライアンス制御を行いながら二つのワークを嵌合させる嵌合装置に関する。

ロボットを用いて組立作業を行う場合、テーブル上の所定の位置にワークを位置決めすると共に、ロボットによってその位置に精度良く他のワークを移動させる必要がある。しかしながら、ロボットに把持されるワークとテーブル上に固定されるワークとの間には位置誤差及び姿勢誤差があり、これらがロボットによる組立作業、特に二つのワークの嵌合作業などに支障を与える場合がある。この問題を解決するために、二つのワークを互いに嵌合させるための嵌合装置では、ロボットアームとハンドとの間にバネやダンパなどを介装させることによりロボットアームの先端に取り付けたハンドの動作に機械的に弾性を持たせ、多少の位置誤差及び姿勢誤差があってもハンドに把持されるワークの位置及び姿勢を自動的に変えて他方のワークに滑らかに嵌合させるようにしたＲＣＣ（遠隔中心コンプライアンス）機構が一般に用いられる。また、特許文献１〜特許文献４などに記載されているように、ロボットアームの動作の制御により、ロボットアームとハンドとの間にバネやダンパなどがあるかのようにロボットを動作させ、ＲＣＣ機構と同様の作用を得るコンプライアンス制御の手法も用いられる。

従来のコンプライアンス制御では、互いに嵌合する二つのワークのうちの一方をロボットアームのハンドにより把持して他方のワークに押し付け、その際にハンドに把持されるワークに作用する力及びモーメントを検出し、ロボットアームの動作を制御して検出される力及びモーメントが目標量になるようにロボットアームの動作を修正しながら嵌合動作を行う。ロボットアームの動作の修正の際には、制御周期毎に、検出された力と目標とする力との差分及び検出されたモーメントと目標とするモーメントとの差分に力制御ゲインというパラメータをかけることによりロボットアームに対する指令速度及び指令角速度を演算し、演算により求められた指令速度及び指令角速度に基づいてハンドに把持されるワークを並進移動及び回転させる。したがって、力制御ゲインが力制御の性能を表し、力制御ゲインの値が大きいほどハンドに把持されるワークの位置及び姿勢の修正が早くなる。

特公平４−４３７４４号公報特公平６−８３９７６号公報特開平５−３８６３７号公報特許第２６０４９２９号

上述したように、力制御ゲインが大きいほどハンドに把持されるワークの位置及び姿勢の修正は短時間で行われるようになる。しかしながら、力制御ゲインを大きくすると、ロボットアームの振動やワーク同士の接触による振動も増幅されやすくなり、ロボットアームの動作が発振しやすくなる。このため、従来のコンプライアンス制御では、この発振限界によって決まる値までしか力制御ゲインを大きくすることができず、ワークやロボットアームの位置や姿勢に初期誤差があるシステムでは、誤差の修正に多大な時間を要していた。

本発明は、上記従来技術に存する問題を解決し、コンプライアンス制御を用いた嵌合装置において、従来よりも高速で安定した嵌合を行うこと可能にすることにある。

上記目的を達成するために、本発明は、ワークを把持するハンドを先端に備えたロボットアームと、ハンドに把持されたワークが受ける力及びモーメントを検出するための力検出器と、ロボットアームの動作を制御するための制御装置とを備え、互いに嵌合する二つのワークのうちの一方を前記ハンドにより把持して嵌合方向に移動させ、前記力検出器の出力に基づいて前記ハンドに把持されるワークの位置及び姿勢を修正するようにコンプライアンス制御を行いながら、前記二つのワークのうちの他方に嵌合させる嵌合装置において、前記制御装置が、前記二つのワークを嵌合させるように動作指令を作成する動作指令作成部と、前記二つのワークの接触中に前記力検出器によって検出される力及びモーメントの最大値又は前記二つのワークが初めて接触したときに前記力検出器によって検出された力及びモーメントの何れかに基づき、前記力検出器によって検出される力及びモーメントが予め定められた閾値以下になるまで、前記嵌合方向に対して垂直な方向に関する前記ハンドの位置と前記嵌合方向に対して垂直な軸線周りの前記ハンドの姿勢とを修正するように前記動作指令作成部によって作成された動作指令を補正する動作指令補正部とを備え、補正後の動作指令に従って前記ロボットアームの動作を制御し前記二つのワークを互いに嵌合させるようにした嵌合装置を提供する。

本発明の嵌合装置では、制御装置の各制御周期において、二つのワークの接触中に力検出器によって検出される力及びモーメントの最大値又は二つのワークが初めて接触したときに力検出器によって検出される力及びモーメントに基づいて、ロボットアームに対する動作指令の補正を行って、力検出器によって検出される力及びモーメントが予め定められた閾値以下になるまで嵌合方向に対して垂直な方向に関するハンド及びワークの位置と嵌合方向に対して垂直な軸線周りのハンド及びワークの姿勢とを修正する。また、ハンドに把持されるワークに作用する力及びモーメントの検出は、制御周期毎に行われるが、検出された力及びモーメントは、予め定められた閾値以下になったか否かを監視するためだけに使用される。したがって、ロボットアームやワークの振動により力検出手段にノイズが乗っても修正動作にほとんど影響を与えず、従来よりも力制御ゲインに大きな値を設定することが可能になる。

前記動作指令補正部は、前記力検出器によって検出される力及びモーメントを小さくするように動作指令を補正することが好ましい。例えば、前記力検出器は、前記嵌合方向に対して垂直であり且つ互いが垂直である２軸方向に作用する力を検出し、前記動作指令補正部は、前記力検出器によって検出される２軸方向の力に基づいて動作指令を補正し、前記嵌合方向に対して垂直な方向に関する前記ハンドの位置を修正する。また、前記力検出器は、前記嵌合方向に対して垂直であり且つ互いが垂直である２軸周りのモーメントを検出し、前記動作指令補正部は、前記力検出器によって検出される２軸周りのモーメントに基づいて動作指令を補正し、前記嵌合方向に対して垂直な軸線周りの前記ハンドの姿勢を修正する。

前記動作指令補正部は、前記力検出器によって検出される力及びモーメントが０になるまで、前記動作指令を補正することが好ましい。
また、前記力検出器は力覚センサとすることができる。
前記力検出器は、前記ロボットアームを駆動するアクチュエータの電流値に基づいて、前記ワークが受ける力及びモーメントを求めてもよい。

なお、本願において、用語「力制御ゲイン」は、以下の式により定義される定数である。
力に関する力制御ゲイン＝指令速度（ｍ／ｓ）／（検出された力−目標力）（Ｋｇｆ）
モーメントに関する力制御ゲイン＝指令角速度（ｒａｄ／ｓ）／（検出されたモーメント−目標モーメント）（Ｋｇｆｍ）

本発明によれば、ロボットアームやワークの振動により力検出手段にノイズが乗っても修正動作に影響を与えることがなく、従来よりも力制御ゲインに大きな値を設定することが可能になるので、ワークやロボットアームに初期誤差があってもこれを従来よりも高速に修正できる。したがって、高速で安定した嵌合作業を行うことが可能になる。

以下、図面を参照して本発明の好ましい実施形態について記載する。
最初に、図１を参照して、本発明の嵌合装置１０の全体構成を説明する。
嵌合装置１０は、複数の軸周りの動作が可能なロボットアーム１２と、力及びモーメントを検出するための力検出器１４と、ロボットアーム１２の動作を制御するための制御装置１６と、ワークＷ１を戴置するためのテーブル１８とを備える。ロボットアーム１２の先端部にはワークＷ２の把持及び解放が可能なハンド２０が設けられている。また、テーブル１８上には、ワークＷ１を固定するためのクランプ装置２２が設けられており、ハンド２０に把持されたワークＷ２に嵌合させるべきワークＷ１を着脱可能に固定できるようになっている。

力検出器１４は、ロボットアーム１２の手首部に取り付けられており、ハンド２０に把持されたワークＷ２が受ける力Ｆ及びモーメントＭを検出できるようになっている。例えば、力検出器１４は、ロボットアーム１２の先端とハンド２０との間に取り付けられており、直交３軸方向の力及び直交３軸周りのモーメントを検出できる６軸力覚センサとすることができる。しかしながら、力検出器１４は、６軸力覚センサに限定されるものではなく、ロボットアーム１２を駆動するためのモータなどのアクチュエータ（図示せず）の電流値に基づいてハンド２０に把持されたワークＷ２が受ける力及びモーメントを推定する装置とすることもできる。

互いに嵌合するワークＷ１、Ｗ２は、たとえば、例えば円筒形状の突出部２４を有するワークＷ２と、円筒形状の突出部２４に対応する円筒形状の嵌合穴２６が形成されたワークＷ１とから構成される。図１に示されている嵌合装置１０では、突出部２４を有するワークＷ２をハンド２０で把持し、テーブル１８に固定されたワークＷ１の嵌合穴２６の中心軸線２８上に突出部２４を整列させて中心軸線２８と平行な嵌合方向にワークＷ２を移動させることにより、ワークＷ１の嵌合穴２６にワークＷ２の突出部２４を挿入し、二つのワークＷ１、Ｗ２を嵌合させる。

図２は、本発明による嵌合装置１０の機能ブロック図であり、嵌合装置１０で使用される制御装置１６の詳細を示している。
制御装置１６は、図２に示されているように、ロボットアーム１２及びハンド２０に対する動作指令を作成する動作指令作成部３０と、動作指令作成部３０によって作成された動作指令を一定条件の下に補正する動作指令補正部３２とを含む。動作指令作成部３０は、予め入力されたプログラム等に従って、互いに嵌合する二つのワークＷ１、Ｗ２のうちの一方のワークＷ２を所定の位置でハンド２０によって把持し、このワークＷ２をテーブル１８上の所定の位置に固定された他方のワークＷ１に嵌合させるように、ロボットアーム１２及びそのハンド２０に対する動作指令、詳細には速度指令及び角速度指令を作成する。また、動作指令補正部３２は、力制御ゲイン部３４と、力及びモーメント保持部３６とによって構成されている。

力及びモーメント保持部３６は、ＲＡＭなどの記憶装置によって実現され、ハンド２０によって把持されるワークＷ２とテーブル１８上に固定されるワークＷ１との接触中に力検出器１４によって検出される力Ｆ及びモーメントＭの最大値Ｆ´、Ｍ´又はハンド２０によって把持されるワークＷ２とテーブル１８上に固定されるワークＷ１とが初めて接触したときに力検出器１４によって検出される力Ｆ及びモーメントＭの値Ｆ０、Ｍ０を記憶又は保持するようになっている。また、力制御ゲイン部３４は、力及びモーメント保持部３６に保持される力Ｆ´、Ｆ０及びモーメントＭ´、Ｍ０とワークＷ１、Ｗ２同士の嵌合時に力検出器１４によって検出されるべき目標力Ｆｄ及び目標モーメントＭｄとの差分を入力すると、この入力に力制御ゲインＧをかけることにより、力検出器１４によって検出される力Ｆ及びモーメントＭを目標力Ｆｄ及び目標モーメントＭｄに近づけるために必要な動作をロボットアーム１２及びハンド２０にさせるための速度指令補正量υｃ及び角速度指令補正量ωｃを出力するようになっている。

動作指令補正部３２は、動作指令作成部３０によって作成される速度指令υ及び角速度指令ωに力制御ゲイン部３４から出力される速度指令補正量υｃ及び角速度指令補正量ωｃを加算することにより、速度指令υ及び角速度指令ωを補正し、補正後の速度指令υ´及び角速度指令ω´に基づいてロボットアーム１２及びそのハンド２０を動作させる。動作指令補正部３２による動作指令の補正は、力検出器１４によって検出される力Ｆ及びモーメントＭが予め定められた閾値ＴＦ、ＴＭ以下になるまで続けられるようになっている。

次に、図３〜図５を参照して、図１及び図２に示されている嵌合装置１０の動作を説明する。以下の説明において、Ｚ軸は、テーブル１８上に固定されるワークＷ１の嵌合穴２６の中心軸線２８の方向を指し、Ｘ軸及びＹ軸は、Ｚ軸に垂直な平面において互いに垂直な方向を指すものとする。
まず、作業者又は他のロボットなどによって嵌合させるべき一対のワークＷ１、Ｗ２のうちの一方のワークＷ１をテーブル１８上の所定の位置に固定する。ロボットアーム１２は、予め定められたプログラム等に基づいて動作指令作成部３０によって作成された動作指令に従って、所定の位置に配置された嵌合相手となる他方のワークＷ２をハンド２０によって把持し、テーブル１８上に固定されたワークＷ１の嵌合穴２６の中心軸線２８とハンド２０によって把持されたワークＷ２の突出部２４の軸線とが整列するように、テーブル１８に固定されるワークＷ１と対向する位置に移動させる。さらに、ロボットアーム１２は、ワークＷ１の嵌合穴２６の中心軸線２８と平行な嵌合方向（すなわち、Ｚ軸方向）にワークＷ２を移動させ、テーブル１８上のワークＷ１に接触させる（ステップＳ１００）。なお、動作指令は、ハンド２０をＸ軸、Ｙ軸及びＺ軸方向に並進移動させるための速度指令υ（υ_X，υ_Y，υ_Z）と、ハンドをＸ軸、Ｙ軸及びＺ軸周りに夫々回転させる角速度指令ω（ω_W，ω_P，ω_R）とを含んでおり、嵌合時の速度指令υ及び角速度指令ωにおいて、嵌合方向の速度指令以外は０となる、すなわち、υ_X＝０、υ_Y＝０、ω_W＝０、ω_P＝０、ω_R＝０となる。

テーブル１８上に固定されたワークＷ１の嵌合穴２６の中心軸線２８とハンド２０によって把持されたワークＷ２の突出部２４の軸線とが正確に整列するように、ワークＷ１及びワークＷ２が配置されている場合、ワークＷ２の突出部２４はワークＷ１の嵌合穴２６に円滑に挿入される。つまり、嵌合中にワークＷ２へ作用する力Ｆ（Ｆ_X，Ｆ_Y，Ｆ_Z）及びモーメントＭ（Ｍ_X，Ｍ_Y，Ｍ_Z）は、嵌合方向成分以外はすべて０になる（すなわち、Ｆ_X＝０，Ｆ_Y＝０，Ｍ_X＝０，Ｍ_Y＝０，Ｍ_Z＝０）。したがって、嵌合中にワークＷ２へ作用すべき目標力Ｆｄ（Ｆｄ_X，Ｆｄ_Y，Ｆｄ_Z）及び目標モーメントＭｄ（Ｍｄ_X，Ｍｄ_Y，Ｍｄ_Z）は、嵌合方向以外ですべて０（すなわち、Ｆｄ_X＝０，Ｆｄ_Y＝０，Ｍｄ_X＝０，Ｍｄ_Y＝０，Ｍｄ_Z＝０）となるように定められる。

ところが、一般に、テーブル１８上に固定されるワークＷ１の位置及び姿勢やハンド２０に把持されるワークＷ２の位置及び姿勢には誤差があるため、テーブル１８上に固定されたワークＷ１の嵌合穴２６の中心軸線２８とハンド２０によって把持されたワークＷ２の突出部２４の軸線とが正確に一直線上に整列していない状態で嵌合動作が行われることが多い。このような場合、ハンド２０に把持されるワークＷ２がテーブル１８上に固定されるワークＷ１に接触すると、ハンド２０に把持されるワークＷ２に力Ｆ及びモーメントＭが作用する（ステップＳ１０２）。例えば、図４（ａ）に示されているように、テーブル１８上に固定されるワークＷ１の嵌合穴２６の中心軸線２８に対して、ハンド２０に把持されるワークＷ２の突出部２４の軸線３８が傾いている場合、両ワークＷ１、Ｗ２が接触すると、図４（ｂ）に示されているように、ハンド２０に把持されるワークＷ２には、嵌合方向に垂直な方向の力Ｆ_X、Ｆ_Yと嵌合方向に垂直な軸線周りのモーメントＭ_X、Ｍ_Yが作用する。力検出器１４が、ワークＷ２が受ける力Ｆ及びモーメントＭを検出すると、制御装置１６は、力検出器に１４よって検出された力Ｆ及びモーメントＭを目標力Ｆｄ及び目標モーメントＭｄに近づけるようにロボットアーム１２及びそのハンド２０の動作を制御する。

本発明の嵌合装置１０では、テーブル１８上に固定されるワークＷ１とハンド２０に把持されるワークＷ２との接触中に力検出器１４によって検出される力Ｆ（Ｆ_X，Ｆ_Y，Ｆ_Z）及びモーメントＭ（Ｍ_X，Ｍ_Y，Ｍ_Z）の最大値が力及びモーメント保持部３６に記憶又は保持されるようになっている。力及びモーメント保持部３６は、制御周期毎に、力検出器１４によって検出された力Ｆ（Ｆ_X，Ｆ_Y，Ｆ_Z）及びモーメントＭ（Ｍ_X，Ｍ_Y，Ｍ_Z）と力及びモーメント保持部３６に記憶又は保持される力Ｆ´（Ｆ´_X，Ｆ´_Y，Ｆ´_Z）及びモーメントＭ´（Ｍ´_X，Ｍ´_Y，Ｍ´_Z）とを対応する各成分について比較し、力検出器１４によって検出された力Ｆ（Ｆ_X，Ｆ_Y，Ｆ_Z）及びモーメントＭ（Ｍ_X，Ｍ_Y，Ｍ_Z）の何れかの成分がその時点で力及びモーメント保持部３６に記憶又は保持される力Ｆ´（Ｆ´_X，Ｆ´_Y，Ｆ´_Z）及びモーメントＭ´（Ｍ´_X，Ｍ´_Y，Ｍ´_Z）の対応する成分よりも大きいときには、記憶又は保持される力Ｆ´（Ｆ´_X，Ｆ´_Y，Ｆ´_Z）及びモーメントＭ´（Ｍ´_X，Ｍ´_Y，Ｍ´_Z）のその成分の値を力検出器１４によって検出された力Ｆ（Ｆ_X，Ｆ_Y，Ｆ_Z）及びモーメントＭ（Ｍ_X，Ｍ_Y，Ｍ_Z）の対応する成分の値に更新し、これを記憶又は保持する。

動作指令補正部３２は、Ｚ軸方向の速度指令υ_Zについて、従来のコンプライアンス制御と同様に、力検出器１４によって検出されるＺ軸方向の力Ｆ_ZとＺ軸方向の目標力Ｆｄ_Zとの差分にＺ軸方向の力についての力制御ゲインＧ_Zをかけることにより速度指令補正量υｃ_Zを演算し、演算により求めた速度指令補正量υｃ_Zを動作指令作成部３０によって作成されたＺ軸方向の速度指令υ_Zに加算してＺ軸方向の速度指令を補正する。すなわち、式（１）に従って、補正後のＺ軸方向の速度指令υ´_Zを求める。
υ´_Z＝（Ｆ_Z−Ｆｄ_Z）×Ｇ_Z＋υ_Z （１）

一方、ワークＷ２の突出部２４やワークＷ１の嵌合穴２６は円筒形状でありＺ軸周りの回転は意味を持たないので、Ｚ軸周りの角速度指令ω_R及び補正後のＺ軸周りの角速度ω´_Rを求める必要はない。しかしながら、ワークＷ２の突出部２４やワークＷ１の嵌合穴２６が非円筒形状である場合には、従来のコンプライアンス制御と同様に以下の式により補正後のＺ軸周りの角速度ω´_Rを求めればよい。
ω´_R＝（Ｍ_Z−Ｍｄ_Z）×Ｇ_R＋ω_R
ここで、Ｇ_RはＺ軸周りのモーメントについての力制御ゲインである。

また、動作指令補正部３２は、Ｘ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、Ｙ軸周りの角速度指令ω_Pについては、力及びモーメント保持部３６に記憶又は保持される力及びモーメントの最大値Ｆ´、Ｍ´と目標力Ｆｄ及び目標モーメントＭｄとの差分に力制御ゲインＧをかけることにより速度指令補正量υｃ_X、υｃ_Y並びに角速度指令補正量ωｃ_W、ωｃ_Pを演算し、演算により求めた速度指令補正量υｃ_X、υｃ_Y並びに角速度指令補正量ωｃ_W、ωｃ_Pをそれぞれ動作指令作成部３０によって作成されたＸ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、Ｙ軸周りの角速度指令ω_Pに加算してＸ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、Ｙ軸周りの角速度指令ω_Pを補正する。補正後のＸ軸方向の速度指令υ´_X、Ｙ軸方向の速度指令υ´_Y、Ｘ軸周りの角速度指令ω´_W、Ｙ軸周りの角速度指令ω´_Pは、Ｘ軸及びＹ軸方向の目標力Ｆｄ_X、Ｆｄ_Y並びにＸ軸及びＹ軸周りのモーメントＭｄ_X、Ｍｄ_Yが何れも０であることを考慮すると、以下の式（２）〜（５）により求められる。
υ´_X＝（Ｆ´_X−Ｆｄ_X）×Ｇ_X＋υ_X＝Ｆ´_X×Ｇ_X （２）
υ´_Y＝（Ｆ´_Y−Ｆｄ_Y）×Ｇ_Y＋υ_Y＝Ｆ´_Y×Ｇ_Y （３）
ω´_W＝（Ｍ´_X−Ｍｄ_X）×Ｇ_W＋ω_W＝Ｍ´_X×Ｇ_W （４）
ω´_P＝（Ｍ´_Y−Ｍｄ_Y）×Ｇ_P＋ω_P＝Ｍ´_Y×Ｇ_P （５）
ここで、Ｇ_X、Ｇ_Yは、それぞれ、Ｘ軸方向の力についての力制御ゲイン、Ｙ軸方向の力についての力制御ゲインであり、Ｇ_W、Ｇ_Pは、それぞれ、Ｘ軸周りのモーメントについての力制御ゲイン、Ｙ軸周りのモーメントについての力制御ゲインである。

なお、嵌合動作は、ハンド２０及びワークＷ２を嵌合方向すなわちＺ軸方向にのみ移動させる動作であり、Ｘ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、及びＹ軸周りの角速度指令ω_Pは何れも０になるので、動作指令補正部３２によって求められた動作指令補正量υｃ_X、υｃ_Y、ωｃ_W、ωｃ_Pが、補正後のＸ軸方向の速度指令υ´_X、Ｙ軸方向の速度指令υ´_Y、Ｘ軸周りの角速度指令ω´_W、Ｙ軸周りの角速度指令ω´_Pとなる。したがって、動作指令補正部３２は、力及びモーメント保持部３６に記憶又は保持される力及びモーメントの最大値Ｆ´、Ｍ´に力制御ゲインＧをかけることにより、Ｘ軸及びＹ軸方向の速度指令並びにＸ軸及びＹ軸周りの角速度指令として、υ´_X、υ´_Y、ω´_W、ω´_Pを求めていることになる。

制御装置１６は、このようにして求めた補正後のＸ軸方向の速度指令υ´_X、Ｙ軸方向の速度指令υ´_Y、Ｚ軸方向の速度指令υ´_Z、Ｘ軸周りの角速度指令ω´_W、Ｙ軸周りの角速度指令ω´_Pに基づいてロボットアーム１２及びそのハンド２０を動作させて、ハンド２０及びワークＷ２の位置及び姿勢を修正させる（ステップＳ１０４）。

次に、制御周期毎に、力検出器１４によって検出されたＸ軸及びＹ軸方向の力Ｆ_X，Ｆ_Y、Ｘ軸及びＹ軸周りのモーメントＭ_X，Ｍ_Yと予め定められた閾値ＴＦ_X，ＴＦ_Y、ＴＭ_X，ＴＭ_Yとを比較し、何れかが予め定められた閾値よりも大きい場合には、Ｆ_X，Ｆ_Y、Ｍ_X，Ｍ_Yの全てが次式（６）を満たすまで、制御周期毎にステップＳ１０４を繰り返し、ハンド２０及びワークＷ２の位置及び姿勢の修正を継続する（ステップＳ１０６）。
Ｆ_X≦ＴＦ_X
Ｆ_Y≦ＴＦ_Y （６）
Ｍ_X≦ＴＭ_X
Ｍ_Y≦ＴＭ_Y
ここで、閾値ＴＦ_X，ＴＦ_Yは、それぞれ、検出されるＸ軸方向の力Ｆ_Xに対する閾値、検出されるＹ軸方向の力Ｆ_Yに対する閾値であり、閾値ＴＭ_X，ＴＭ_Yは、それぞれ、検出されるＸ軸周りのモーメントＭ_Xに対する閾値、検出されるＹ軸周りのモーメントＭ_Yに対する閾値である。

一方、力検出器１４によって検出された力Ｆ_X，Ｆ_Y及びモーメントＭ_X，Ｍ_Yの各成分が全て予め定められた閾値ＴＦ_X，ＴＦ_Y，ＴＭ_X，ＴＭ_Y以下になり、式（６）を満たすようになると、ハンド２０及びワークＷ２の位置及び姿勢の誤差の修正が完了したと判断される（ステップＳ１０８）。そして、ハンド２０及びワークＷ２の位置及び姿勢の修正動作が完了すると、ハンド２０に把持されるワークＷ２の突出部２４の軸線３８がテーブル１８に固定されたワークＷ１の嵌合穴２６の中心軸線２８に整列して一直線上に配置された状態になるので、この位置及び姿勢のままハンド２０及びワークＷ２を嵌合方向に移動させれば、ハンド２０に把持されるワークＷ２の突出部２４をテーブル１８に固定されるワークＷ１の嵌合穴２６に円滑に挿入することができ、嵌合作業が終了する（ステップＳ１１０）。

上述した本発明の嵌合装置１０の動作では、力及びモーメント保持部３６に、ワークＷ１とワークＷ２との接触中に力検出器１４によって検出された力Ｆ及びモーメントＭの最大値Ｆ´（Ｆ´_X，Ｆ´_Y，Ｆ´_Z）、Ｍ´（Ｍ´_X，Ｍ´_Y，Ｍ´_Z）が記憶又は保持されると説明した。しかしながら、力及びモーメント保持部３６は、ワークＷ１とワークＷ２とが初めて接触したときに力検出器１４によって検出される力Ｆ０（Ｆ０_X，Ｆ０_Y，Ｆ０_Z）、及びモーメントＭ０（Ｍ０_X，Ｍ０_Y，Ｍ０_Z）を記憶又は保持するようになっていてもよい。なお、二つのワークＷ１、Ｗ２が接触していないときには力Ｆ及びモーメントＭはほぼ０となるので、力検出器に１４よって検出される力Ｆ（Ｆ_X，Ｆ_Y，Ｆ_Z）又はモーメント（Ｍ_X，Ｍ_Y，Ｍ_Z）の成分が予め定められた値以上になったことを検出することによって、二つのワークＷ１、Ｗ２が接触したことを検出することができる。

このように、力及びモーメント保持部３６がワークＷ１とワークＷ２とが初めて接触したときに力検出器１４によって検出される力Ｆ０及びモーメントＭ０を記憶又は保持するようになっている場合の動作手順が図５に示されている。図５に示されている動作手順は、図３に示されている動作手順のステップＳ１０４に代えてステップＳ１１２が行われる点において、図３に示されている動作手順と異なっており、その他の手順は同様である。

ステップＳ１００によりワークＷ１、Ｗ２同士が接触し、ハンド２０によって把持されるワークＷ２に力Ｆ及びモーメントＭが作用すると、力検出器１４によってワークＷ２に作用する力Ｆ及びモーメントＭが検出される（ステップＳ１０２）。すると、力及びモーメント保持部３６は、ワークＷ１とＷ２とが初めて接触したときに力検出器１４によって検出された力Ｆ０（Ｆ０_X，Ｆ０_Y，Ｆ０_Z）及びモーメントＭ０（Ｍ０_X，Ｍ０_Y，Ｍ０_Z）を記憶し、以降、これを保持し続ける。

動作指令補正部３２は、ステップＳ１０４と同様にして、力検出器１４によって検出されるＺ軸方向の力Ｆ_ZとＺ軸方向の目標力Ｆｄ_Zとの差分に基づいて、動作指令作成部３０によって作成されたＺ軸方向の速度指令υ_Zを補正し、式（１）に従って、補正後のＺ軸方向の速度指令υ´_Zを求める。なお、ワークＷ２の突出部２４やワークＷ１の嵌合穴２６は円筒形状でありＺ軸周りの回転は意味を持たないので、Ｚ軸周りの角速度指令ω_R及び補正後のＺ軸周りの角速度ω´_Rを求める必要はない。しかしながら、ワークＷ２の突出部２４やワークＷ１の嵌合穴２６が非円筒形状である場合には、図３に示されている手順の場合と同じようにして補正後のＺ軸周りの角速度ω´_Rを求めればよい。

一方、動作指令補正部３２は、Ｘ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、Ｙ軸周りの角速度指令ω_Pについて、力及びモーメント保持部３６に記憶又は保持される力Ｆ０及びモーメントＭ０と目標力Ｆｄ及び目標モーメントＭｄとの差分に力制御ゲインＧをかけることにより動作指令補正量υｃ_X、υｃ_Y、ωｃ_W、ωｃ_Pを演算し、演算により求めた動作指令補正量υｃ_X、υｃ_Y、ωｃ_W、ωｃ_Pをそれぞれ動作指令作成部３０によって作成されたＸ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、Ｙ軸周りの角速度指令ω_Pに加算してＸ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、Ｙ軸周りの角速度指令ω_Pを補正する。補正後のＸ軸方向の速度指令υ´_X、Ｙ軸方向の速度指令υ´_Y、Ｘ軸周りの角速度指令ω´_W、Ｙ軸周りの角速度指令ω´_Pは、Ｘ軸及びＹ軸方向の目標力Ｆｄ_X、Ｆｄ_Y並びにＸ軸及びＹ軸周りのモーメントＭｄ_X、Ｍｄ_Yが何れも０であることを考慮すると、以下の式（２´）〜（５´）により求められる。
υ´_X＝（Ｆ０_X−Ｆｄ_X）×Ｇ_X＋υ_X＝Ｆ０_X×Ｇ_X （２´）
υ´_Y＝（Ｆ０_Y−Ｆｄ_Y）×Ｇ_Y＋υ_Y＝Ｆ０_Y×Ｇ_Y （３´）
ω´_W＝（Ｍ０_X−Ｍｄ_Y）×Ｇ_W＋ω_W＝Ｍ０_X×Ｇ_W （４´）
ω´_P＝（Ｍ０_Y−Ｍｄ_Y）×Ｇ_P＋ω_P＝Ｍ０_Y×Ｇ_P （５´）
ここで、Ｇ_X、Ｇ_Yは、それぞれ、Ｘ軸方向の力についての力制御ゲイン、Ｙ軸方向の力についての力制御ゲインであり、Ｇ_W、Ｇ_Pは、それぞれ、Ｘ軸周りのモーメントについての力制御ゲイン、Ｙ軸周りのモーメントについての力制御ゲインである。

また、Ｘ軸方向の速度指令υ_X、Ｙ軸方向の速度指令υ_Y、Ｘ軸周りの角速度指令ω_W、及びＹ軸周りの角速度指令ω_Pは何れも０になるので、動作指令補正部３２によって求められた動作指令補正量υｃ_X、υｃ_Y、ωｃ_W、ωｃ_Pが、補正後のＸ軸方向の速度指令υ´_X、Ｙ軸方向の速度指令υ´_Y、Ｘ軸周りの角速度指令ω´_W、Ｙ軸周りの角速度指令ω´_Pとなる。したがって、動作指令補正部３２は、力及びモーメント保持部３６に記憶又は保持される力Ｆ０及びモーメントＭ０に力制御ゲインＧをかけることにより、Ｘ軸及びＹ軸方向の速度指令並びにＸ軸及びＹ軸周りの角速度指令として、υ´_X、υ´_Y、ω´_W、ω´_Pを求めていることになる。

制御装置１１６は、このようにして求めた補正後のＸ軸方向の速度指令υ´_X、Ｙ軸方向の速度指令υ´_Y、Ｚ軸方向の速度指令υ´_Z、Ｘ軸周りの角速度指令ω´_W、Ｙ軸周りの角速度指令ω´_Pに基づいてロボットアーム１２及びそのハンド２０を動作させて、ハンド２０及びワークＷ２の位置及び姿勢を修正させる（ステップＳ１１２）。

次に、制御周期毎にワークＷ２が受ける力Ｆ及びモーメントＭを力検出器１４によって検出し、検出されたＸ軸及びＹ軸方向の力Ｆ_X，Ｆ_Y、Ｘ軸及びＹ軸周りのモーメントＭ_X，Ｍ_Yと予め定められた閾値ＴＦ_X，ＴＦ_Y、ＴＭ_X，ＴＭ_Yとを比較し、何れかが予め定められた閾値よりも大きい場合には、Ｆ_X，Ｆ_Y、Ｍ_X，Ｍ_Yの全てが式（６）を満たすまで、制御周期毎にステップＳ１１２を繰り返し、ハンド２０及びワークＷ２の位置及び姿勢の修正を継続する（ステップＳ１０６）。

一方、力検出器１４によって検出された力Ｆ_X，Ｆ_Y及びモーメントＭ_X，Ｍ_Yの各成分が全て予め定められた閾値ＴＦ_X，ＴＦ_Y，ＴＭ_X，ＴＭ_Y以下になり、式（６）を満たすようになると、ハンド２０及びワークＷ２の位置及び姿勢の誤差の修正が完了したと判断される（ステップＳ１０８）。そして、ハンド２０及びワークＷ２の位置及び姿勢の修正が完了すると、ハンド２０に把持されるワークＷ２の突出部２４の軸線３８がテーブル１８に固定されたワークＷ１の嵌合穴２６の中心軸線２８に整列して一直線上に配置された状態になるので、この位置及び姿勢のままハンド２０を嵌合方向に移動させれば、ハンド２０に把持されるワークＷ２の突出部２４をテーブル１８に固定されるワークＷ１の嵌合穴２６に円滑に挿入することができ、嵌合作業が終了する（ステップＳ１１０）。

次に、図６及び図７を参照して、ワークＷ１、Ｗ２に姿勢誤差がある場合を例にして、本発明による効果を説明する。
図６及び図７には、それぞれ従来のコンプライアンス制御のように力検出器１４によって検出されたモーメントＭに基づいてワークＷ１に対するワークＷ２のＸ軸及びＹ軸周りの姿勢誤差を修正した場合と本発明により力検出器１４によって検出されたモーメントの最大値Ｍ´に基づいてワークＷ１に対するワークＷ２のＸ軸及びＹ軸周りの姿勢誤差を修正した場合において力検出器１４によって検出されるモーメントＭ及びハンド２０の姿勢変化量の時間変化を表すグラフが示されている。

従来方式を表す図６では、ワークＷ２などの振動による発振を防止するために力制御ゲインＧが０．０３（ｒａｄ／Ｋｇｆｍｓ）に設定されているのに対して、本発明では従来よりも発振限界が高くなるので図７では、力制御ゲインＧが図６の場合の力制御ゲインの値の３倍の０．０９（ｒａｄ／Ｋｇｆｍｓ）に設定されている。また、図６及び図７では、何れの場合も、ハンド２０に把持されるワークＷ２の突出部２４の軸線３８とテーブル１８に固定されるワークＷ１の嵌合穴２６の中心軸線２８とが１０ｍｒａｄだけずれた状態から嵌合動作が開始され、１．３（ｓ）以降にワークＷ２の姿勢誤差に起因するモーメントＭが発生し、１．５（ｓ）以降に姿勢誤差の修正が行われている。

従来方式を表す図６では、制御周期毎にその時点で力検出器１４によって検出されたモーメントＭの大きさから角速度指令ωを求めているのに対して、本発明を表す図７では、接触中に力検出器１４によって検出されたモーメントの最大値Ｍ´から角速度指令を求めている。したがって、図７では、１．７（ｓ）でモーメントＭが最大となった以降は、この最大値Ｍ´から角速度指令を演算し、力検出器１４によって検出されるモーメントＭが０になる２．８（ｓ）までその角速度指令を使用し続けている。

このように、本発明を表す図７では、従来方式を表す図６の場合の力制御ゲインＧが３倍に設定されている結果、ワークＷ２の姿勢誤差の修正は、図６の場合の約３分の１の時間で完了していることが分かる。

本発明の嵌合装置１０では、制御装置１６が、制御周期において、二つのワークＷ１、Ｗ２の接触中に力検出器１４によって検出される力及びモーメントの最大値Ｆ´、Ｍ´又は二つのワークＷ１、Ｗ２が初めて接触したときに力検出器１４によって検出される力Ｆ０及びモーメントＭ０に基づいて、ロボットアーム１２に対する動作指令の補正を行って、力検出器１４によって検出される力Ｆ及びモーメントＭが予め定められた閾値ＴＦ、ＴＭ以下になるまで嵌合方向に対して垂直な方向に関するハンド２０及びワークＷ２の位置と嵌合方向に対して垂直な軸線周りのハンド２０及びワークＷ２の姿勢とを修正する。また、ハンド２０に把持されるワークＷ２に作用する力Ｆ及びモーメントＭの検出は、制御周期毎に行われるが、検出された力Ｆ及びモーメントＭは、予め定められた閾値ＴＦ、ＴＭ以下になったか否かを監視するためだけに使用される。すなわち、力検出器１４によって検出される力Ｆ及びモーメントＭのデータは直接的にフィードバック制御に用いられないようになっている。したがって、ロボットアーム１２やワークＷ２の振動により力検出器１４にノイズが乗っても、ワークＷ１に対するワークＷ２の位置誤差及び姿勢誤差の修正動作にほとんど影響を与えず、従来よりも力制御ゲインＧに大きな値を設定することが可能になるので、ワークＷ１、Ｗ２やロボットアーム１２に初期誤差があってもこれを安定に且つ従来よりも高速に修正することができる。

以上、図示されている実施形態に基づいて、本発明を説明したが、本発明は図示及び説明された実施形態に限定されるものではない。例えば、上述の説明では、各成分に分けて、嵌合方向に垂直な２軸方向（上記説明では、Ｘ軸及びＹ軸）に作用する力Ｆに基づいて位置誤差を修正し、嵌合方向に垂直な２軸周り（上記説明では、Ｘ軸及びＹ軸周り）に作用するモーメントＭに基づいて姿勢誤差を修正している。しかしながら、このように各成分に分けて位置誤差及び姿勢誤差を修正する必要はなく、ワークＷ１、Ｗ２同士が接触した瞬間にＸ軸方向及びＹ軸方向にワークＷ２が受ける力Ｆ_X、Ｆ_Yの合力をＦＲとしたときにＦＲの方向をＸ´とし、Ｘ´方向の速度指令υを力制御ゲインＧから求めると共に、ワークＷ１、Ｗ２同士が接触した瞬間にワークＷ２が受けるＸ軸及びＹ軸周りのモーメントＭ_X、Ｍ_Yの合成モーメントをＭＲとしたときにモーメントＭＲの回転軸方向をＸ″とし、Ｘ″周りの角速度指令ωを求めるようにしてもよい。

本発明の嵌合装置の全体構成を示す斜視図である。本発明による嵌合装置の機能ブロック図である。図１及び図２に示されている嵌合装置による嵌合動作の手順を示すフローチャートである。図１及び図２に示されている嵌合装置による嵌合動作の手順を示す線図である。図１及び図２に示されている嵌合装置による別な形態の嵌合動作の手順を示すフローチャートである。従来のコンプライアンス制御下における、ワークが受けるモーメント及びワークの姿勢の経時変化を示すグラフである。本発明の嵌合装置で採用するコンプライアンス制御下における、ワークが受けるモーメント及びワークの姿勢の経時変化を示すグラフである。

符号の説明

１０嵌合装置
１２ロボットアーム
１４力検出器
１６制御装置
２０ハンド
２４突出部
２６嵌合穴
２８中心軸線
３０動作指令作成部
３２動作指令補正部
３４力制御ゲイン部
３６力及びモーメント保持部
３８軸線

Claims

ワークを把持するハンドを先端に備えたロボットアームと、ハンドに把持されたワークが受ける力及びモーメントを検出するための力検出器と、ロボットアームの動作を制御するための制御装置とを備え、互いに嵌合する二つのワークのうちの一方を前記ハンドにより把持して嵌合方向に移動させ、前記力検出器の出力に基づいて前記ハンドに把持されるワークの位置及び姿勢を修正するようにコンプライアンス制御を行いながら、前記二つのワークのうちの他方に嵌合させる嵌合装置において、
前記制御装置が、前記二つのワークを嵌合させるように動作指令を作成する動作指令作成部と、前記二つのワークの接触中に前記力検出器によって検出される力及びモーメントの最大値又は前記二つのワークが初めて接触したときに前記力検出器によって検出された力及びモーメントの何れかに基づき、前記力検出器によって検出される力及びモーメントが予め定められた閾値以下になるまで、前記嵌合方向に対して垂直な方向に関する前記ハンドの位置と前記嵌合方向に対して垂直な軸線周りの前記ハンドの姿勢とを修正するように前記動作指令作成部によって作成された動作指令を補正する動作指令補正部とを備え、補正後の動作指令に従って前記ロボットアームの動作を制御し前記二つのワークを互いに嵌合させることを特徴とした嵌合装置。
前記動作指令補正部は、前記力検出器によって検出される力及びモーメントを小さくするように動作指令を補正する、請求項１に記載の嵌合装置。
前記力検出器は、前記嵌合方向に対して垂直であり且つ互いが垂直である２軸方向に作用する力を検出し、前記動作指令補正部は、前記力検出器によって検出される２軸方向の力に基づいて動作指令を補正し、前記嵌合方向に対して垂直な方向に関する前記ハンドの位置を修正する、請求項２に記載の嵌合装置。
前記力検出器は、前記嵌合方向に対して垂直であり且つ互いが垂直である２軸周りのモーメントを検出し、前記動作指令補正部は、前記力検出器によって検出される２軸周りのモーメントに基づいて動作指令を補正し、前記嵌合方向に対して垂直な軸線周りの前記ハンドの姿勢を修正する、請求項２に記載の嵌合装置。
前記力検出器は力覚センサである、請求項１から請求項４の何れか一項に記載の嵌合装置。
前記力検出器は、前記ロボットアームを駆動するアクチュエータの電流値に基づいて、前記ワークが受ける力及びモーメントを求める、請求項１から請求項５の何れか一項に記載の嵌合装置。