JP4837113B2 - ロボットを用いた嵌合装置 - Google Patents

Description

本発明は、ワークを対象物の穴に挿入する等の嵌合作業を、ロボットを用いて行う嵌合装置に関する。

ロボットを用いた嵌合装置を用いて、該ロボットの力制御によって嵌合作業を行う場合、ワークの姿勢誤差の修正は図７及び図８に示すような手順で行うことができる。すなわち、例えば図８（ａ）に示すように円柱状のワークＷ２をロボットで把持して嵌合方向Ｄ１に沿って移動させ、対応する嵌合穴Ｈ１を有するワークＷ１に嵌合させる場合、先ずワーク同士を接触させて両ワーク間の姿勢誤差に起因するモーメントＭを検出する（図７のステップＳ１０１、図８（ｂ））。次に、モーメントＭが小さくなるように（或いはゼロに近付くように）ロボットの力制御を行いワークＷ２の姿勢を修正する（図７のステップＳ１０２、図８（ｃ））。次にワークＷ２がワークＷ１の嵌合穴Ｈ１に対して所定の深さまで挿入されたかを判定し（図７のステップＳ１０３）、所定深さまで挿入されていなければステップＳ１０２に戻る。一方、ワークＷ２が所定深さまで挿入されていれば（図８（ｄ））、嵌合完了と判断して処理を終了する（図７のステップＳ１０４）。

なお図７及び図８の例は姿勢誤差の修正に関するものであるが、位置誤差の修正に関しても同様の考え方が適用できる。ロボットの制御においては、ワーク同士に働く力及びモーメントと目標力及び目標モーメントとの差分に力制御ゲインというパラメータ量を乗算して指令速度及び角速度を計算する。力及びモーメントの検出やそれに応じた速度及び角速度指令量の計算は、制御周期毎に行われる。

上記の技術に加え、特許文献１には、把持した挿入部品が被挿入部品に接触したことを検出すると、挿入部品の把持力を弱め、把持部を平面内で移動して穴部の正確な位置を探索して部品の挿入位置を補正し、部品の把持力を再調整し、該部品を該穴部に挿入するという技術が記載されている。接触を認識したら被挿入部品の穴の位置を探索することで、挿入部品の位置が被挿入部品穴の位置から大幅にずれていても正しい穴の位置を探索することができるとされている。

また特許文献２には、並進成分だけでなく姿勢成分の探索動作を行う技術が記載されている。姿勢が大幅にずれているためにモーメントが検出されにくい状況においても、積極的に姿勢を変化させることでワークの適正な姿勢を探索することができるとされている。

さらに特許文献３には、大きさと方向が周期的に変化する振動力を嵌合部品に付加し、嵌合途中での噛付き解消を企図したロボット制御システムが開示されている。

特開平８−１６８９２７号公報 特開２００４−１６７６５１号公報 特開２００８−２６４９１０号公報

従来の制御方式には、姿勢誤差により生じるモーメントが小さいため、該モーメントに基づくワークの姿勢修正が難しいという問題がある。例えば図８（ｂ）に示すように、ワークＷ２の姿勢誤差に起因するモーメントＭは、ワークＷ２に作用する力Ｆ１とＦ２の合力により発生するモーメントであるが、力Ｆ１及びＦ２は概ね相反する向きに作用するため、この合力は各々の力よりも小さくなり、この小さな合力から得られるモーメントも結果として比較的小さいものとなる。つまり、このような小さいモーメントではワークの姿勢修正には不十分となる場合がある。特にロボットや嵌合ワークの剛性が低い場合、ワークの姿勢修正に必要なモーメントが検出されにくいという問題は顕著になる。一方、ワーク間の押付力を高めることで上記モーメントを大きくすることは可能ではあるが、同時にワークを損傷させる可能性も高まってしまう。

特許文献２は、上記問題を解決するための１つの方法を開示している。図９の４つのグラフは、特許文献２に記載の方法で探索動作をさせた場合の嵌合方向（ロボット進行方向）の位置（図９（ａ））、ワークに作用する嵌合方向（ワーク押付け方向）の力（図９（ｂ））、ある方向回りのロボットの姿勢（図９（ｃ））及びモーメント検出値（図９（ｄ））の時間変化の一例を表す。ここでは簡単のため、姿勢成分の１成分のみに着目して考察する。図９（ｃ）からわかるように、この例では押付動作を行いながらロボットの姿勢を周期的に変化させているが、姿勢が適正であるとき（図９（ａ）のＬ１部）は嵌合が進み、適正でないとき（図９（ａ）のＬ２部）は嵌合が進まない。従って、より短時間で嵌合作業を行え、さらに図９（ｂ）のような力がワークに周期的に作用する頻度をより低減できる技術の提供が望まれる。

同様に特許文献３の技術についても、往復動作をするのみであるため大幅に時間を要したり、ワークに余計に力を加えてしまったりすることが考えられる。

そこで本発明は、嵌合方向への押付により十分なモーメントが検出できない状況下でも、ワークを傷付けずに嵌合作業が行える嵌合装置及び嵌合方法を提供することを目的とする。

上記目的を達成するために、請求項１に記載の発明は、嵌合ワークを把持したロボットにより、該嵌合ワークを定位置に設置された被嵌合ワークに嵌合する嵌合装置において、前記嵌合ワークに作用する力を測定する力測定部と、前記被嵌合ワークに対する前記嵌合ワークの嵌合が進行しているか否かを判断する嵌合進行判断部と、前記嵌合進行判断部により嵌合が進行していないと判断された場合に、前記嵌合ワークの現在の姿勢を変化させ、前記嵌合ワークの姿勢を変化させている間、前記力測定部により得られた嵌合方向の力が所定の閾値を下回ったとき又は前記嵌合ワークの嵌合方向の速度が所定の閾値を上回ったときと、前記力が所定の閾値を上回ったとき又は前記速度が所定の閾値を下回ったときとの間の前記嵌合ワークの姿勢を適正姿勢と判断するワーク姿勢探索部と、前記ワーク姿勢探索部により探索された前記嵌合ワークの適正姿勢を用いて、前記嵌合ワークを把持したロボットに嵌合動作を継続させる嵌合動作指示部と、を備えることを特徴とする嵌合装置を提供する。

請求項２に記載の発明は、請求項１に記載の嵌合装置において、前記ワーク姿勢探索部は、各々が前記嵌合ワークの嵌合方向と直交しかつ互いに直交する２方向のうち、少なくとも１つの方向回りについて前記嵌合ワークの姿勢変化を行わせる、嵌合装置を提供する。

請求項３に記載の発明は、請求項１又は２に記載の嵌合装置において、前記ワーク姿勢探索部は、前記嵌合ワークに設定された制御点を中心に前記嵌合ワークの姿勢を所定角度だけ往復変化させ、該往復変化の間に前記力測定部によって得られた嵌合方向の力、又は前記嵌合ワークの嵌合方向の速度に基づいて前記嵌合ワークの適正姿勢を探索する、嵌合装置を提供する。

請求項４に記載の発明は、請求項１〜３のいずれか１項に記載の嵌合装置において、前記力測定部は、前記嵌合ワークに作用する力及びモーメントを測定するように構成され、前記嵌合装置は、前記モーメントが小さくなる方向に前記嵌合ワークの姿勢を力制御によって修正する力制御部をさらに有する、嵌合装置を提供する。

請求項５に記載の発明は、嵌合ワークを把持したロボットにより、該嵌合ワークを定位置に設置された被嵌合ワークに嵌合する嵌合方法において、前記嵌合ワークを前記被嵌合ワークに押付けるステップと、前記嵌合ワークに作用する力を測定するステップと、前記被嵌合ワークに対する前記嵌合ワークの嵌合が進行しているか否かを判断するステップと、前記被嵌合ワークに対する前記嵌合ワークの嵌合が進行していないと判断された場合に、前記嵌合ワークの現在の姿勢を変化させ、前記嵌合ワークの姿勢を変化させている間、前記嵌合ワークに作用する嵌合方向の力が所定の閾値を下回ったとき又は前記嵌合ワークの嵌合方向の速度が所定の閾値を上回ったときと、前記力が所定の閾値を上回ったとき又は前記速度が所定の閾値を下回ったときとの間の前記嵌合ワークの姿勢を適正姿勢と判断するステップと、探索された前記嵌合ワークの適正姿勢を用いて、前記嵌合ワークを把持したロボットに嵌合動作を継続させるステップと、を備えることを特徴とする嵌合方法を提供する。

本発明によれば、嵌合前の初期許容姿勢誤差を大きくすることができる。また無駄な探索動作をなくし、嵌合に必要な時間を短縮できる。さらに、探索した適正姿勢を用いて嵌合作業が行われるので、弱い押付力でも嵌合でき、さらにはロボットや嵌合ワークの剛性が低くても嵌合できるようになる。

本発明の実施形態に係る嵌合装置の概略構成を示す図である。 本発明に係る嵌合作業の流れを示すフローチャートである。 姿勢誤差に起因して嵌合ワークに作用する力及びモーメントを説明する図である。 嵌合ワークの適正姿勢の探索動作の一例を示す図である。 嵌合ワークの適正姿勢の探索動作の他の例を示す図である。 図９（ｂ）の部分拡大図であって、嵌合ワークが適正姿勢となる時間帯を説明するグラフである。 従来技術に係る嵌合作業の流れを示すフローチャートである。 図７に示すフローチャートに従って嵌合作業を行ったときのワークの位置関係を示す図であって、（ａ）嵌合ワークを被嵌合ワークに接近させている状態を示す図であり、（ｂ）嵌合ワークが被嵌合ワークに当接して、姿勢誤差に起因する力及びモーメントが発生している状態を示す図であり、（ｃ）上記モーメントを小さくするように力制御を行っている状態を示す図であり、（ｄ）嵌合ワークと被嵌合ワークとの嵌合が完了した状態を示す図である。 従来技術に係る方法によって嵌合作業を行ったときの各パラメータの時間変化を示す図であって、（ａ）ロボット（嵌合ワーク）の進行方向位置を示すグラフであり、（ｂ）嵌合ワークに作用する押付け方向の力を示すグラフであり、（ｃ）ロボット（嵌合ワーク）の姿勢を示すグラフであり、（ｄ）嵌合ワークに作用するモーメントを示すグラフである。

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

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

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

なお本実施形態では、ロボット１１及びテーブル１８はそれぞれ床に固定されているものとし、また嵌合穴２６の中心軸２８に平行な軸をＺ軸とし、該Ｚ軸に垂直かつ互いに直交する並進二方向をそれぞれＸ軸、Ｙ軸とする。また本実施形態では、本願発明に係る嵌合進行判断部、ワーク姿勢探索部及び嵌合動作指示部の機能はいずれも制御装置１６が担うものとする。

本発明では、後述する嵌合進行を円滑に実行するのに適した嵌合ワークの姿勢、すなわち適正姿勢を判定することができる。つまり本発明では、ワークの適正姿勢が得られたら姿勢を探索する動作を中断させ、姿勢の探索動作をしない従来の力制御動作（図７のステップＳ１０２）へ移行させることができる。しかし本発明では探索動作によって適正な姿勢が得られているので、力制御動作は必須ではなく、適正姿勢を維持したまま単純にワークの押込み動作を行ってもよい。以下、図２のフローチャートを用いて本発明に係る嵌合作業の流れを説明する。

先ずステップＳ１において、ワークＷ１とワークＷ２とを接触させる。具体的には、予め定められたプログラム等に基づいて制御装置１６から送られる動作指令に従って、ワークＷ２をロボットアーム１２に取り付けられたハンド２０によって把持し、テーブル１８上に固定されたワークＷ１の嵌合穴２６の中心軸線２８とハンド２０によって把持されたワークＷ２の突出部２４の軸線とが整列するように、テーブル１８に固定されるワークＷ１と対向する位置に移動させる。さらに、ロボットアーム１２は、ワークＷ１の嵌合穴２６の中心軸線２８と平行な嵌合方向（Ｚ軸方向）にワークＷ２を移動させ、テーブル１８上のワークＷ１に接触させる。

一般にワークＷ２の位置及び姿勢には誤差があるため、ワークＷ１の嵌合穴２６の中心軸線２８とハンド２０によって把持されたワークＷ２の突出部２４の軸線とが正確に一直線上に整列していない状態で嵌合動作が行われることが多い。このような場合に、ワークＷ２をワークＷ１に向けて（嵌合方向に）押付けていくと（ステップＳ２）、ハンド２０に把持されたワークＷ２に力Ｆ１、Ｆ２及びモーメントＭが作用する（図３参照）。詳細には、図３に示されているように、ワークＷ１の嵌合穴２６の中心軸線２８に対して、ワークＷ２の突出部２４の軸線３６が傾いている場合に両ワークが接触すると、ワークＷ２には、ワークＷ１との接触点３２、３４に生じる力Ｆ１、Ｆ２と、嵌合方向に垂直な軸線周りのモーメントＭが作用する。

次のステップＳ３では、この力及びモーメントを力検出器１４によって検出する。詳細には、図３に示すように、ワークＷ２に作用する力Ｆ１、Ｆ２を検出するとともに、ワークＷ２上に設定された制御点Ｐ回りのモーメントを、力Ｆ１、Ｆ２の大きさ、Ｆ１、Ｆ２の作用点３２、３４の位置等に基づいて求め又は検出する。なお制御点Ｐは、図３では略円柱状の嵌合ワークＷ２の嵌合方向側端面（円形端面）の中心に定められているが、これは好適な一例であってこれに限定されるものではない。但し制御点は、嵌合ワークの中心軸線３６（ワークが角柱、楕円柱等であればその端面の重心を通る軸線）上又はその近傍に設定されることが好ましい。また制御点回りのモーメントを求める関係上、制御点は、力Ｆ１、Ｆ２の作用点３２、３４に近い方が好ましい。例えば制御点Ｐは、ワークの嵌合方向側端面３０と平行であって該端面３０から嵌合深さＤの１／２、１／３又は１／４だけ離れて設定された嵌合ワーク内の境界面３８と端面３０とで画定されるワーク内の領域に位置し、かつ中心軸線３６（ワークが角柱、楕円柱等であればその端面の重心を通る軸線）上又はその近傍の点として設定することができる。

次のステップＳ４では、力検出器１４が検出した力及びモーメントに基づいて、制御装置１６が、該力及びモーメントを目標力及び目標モーメントに近づけるようにロボットアーム１２及びハンド２０の動作を制御（力制御）する。

ここでステップＳ４の詳細について説明する。上述のように、嵌合ワークＷ２を嵌合方向（Ｚ方向）に押し付け、ワークＷ２に作用する力及びモーメントの値に基づきロボットの各方向の速度及び角速度を制御する。以下の式（１）〜（５）に基づき制御点の位置を制御する。

ここで、式（１）〜（３）の左辺はそれぞれＸ、Ｙ、Ｚ方向における速度指令値を意味し、式（４）及び（５）の左辺はそれぞれＸ軸及びＹ軸回りの角速度指令値を意味する。また、Ｆ_X、Ｆ_Y及びＦ_ZはそれぞれＸ、Ｙ及びＺ方向における力を意味し、Ｍ_X及びＭ_YはそれぞれＸ軸及びＹ軸回りのモーメントの検出値を意味する。さらに、Ｇ_X、Ｇ_Y、Ｇ_Z、Ｇ_W及びＧ_PはそれぞれＸ、Ｙ、Ｚ、Ｗ及びＰ方向における力制御ゲインを意味し、Ｆ_d及びｖ_dはそれぞれ目標力及び目標速度を表す。これらの計算は計算周期毎に行われる。

式（１）〜（５）が示すように、力やモーメントが検出されれば位置や姿勢を修正できるが、逆に検出値がゼロに近い場合は位置や姿勢を修正することが難しい。そこで、式（４）及び（５）を、それぞれ以下の式（６）及び（７）のように修正し、ゼロでないモーメント指令や角速度指令を追加した探索動作を行う。

式（６）及び（７）において、Ｍ_Xd及びＭ_YdはそれぞれＸ軸及びＹ軸回りの目標モーメントを意味し、ω_Xd及びω_YdはそれぞれＸ軸及びＹ軸回りの目標角速度を意味する。安全のため、探索動作の符号は下記の条件i）又はii）の何れかを満たしたら反転することが好ましい。
i）嵌合開始時と比較して、制御点における嵌合ワークの姿勢が、ユーザの設定した上限角度を超えて変化した場合
ii）嵌合ワークに作用するモーメントが、ユーザの設定した目標モーメントになった場合

ステップＳ１〜Ｓ４までは、概ね従来技術と同様の考え方でよい。そして次のステップＳ５でワークＷ１がワークＷ２に対して所定の深さまで挿入された（図８（ｄ）の状態）か否かを判定し、所定深さまで挿入されていれば嵌合完了と判断して（ステップＳ６）処理は終了する。一方所定深さまで挿入されていない場合は、ステップＳ７に進み、ステップＳ７の判定結果によってはワーク姿勢の探索動作を行い、適正姿勢が得られたら探索動作を停止して再び力制御動作へ移行する。

ステップＳ７では、後述する姿勢探索動作（ステップＳ８）に移行すべきか否かの判定を行う。具体例として、下記３つの条件iii）〜v）の少なくとも１つを選択できる。

iii）ワーク同士が初めて接触したとき、つまり次式（８）を初めて満たしたとき。但し式（８）におけるＣ_Fは、接触を認識するための力の閾値である。

iv）嵌合が進行しているかどうかを判断する嵌合進行判断部により、嵌合が進行していないと判定されたとき。すなわち、以下の式（９）〜（１４）の条件式のうち選択した少なくとも１つの条件式を満たしたときである。

ここで、Ｔ_F、Ｔ_Mはそれぞれ力及びモーメントが目標値に近づいたことを判断するための閾値を意味し、Ｔ_v及びＴ_ωは嵌合が進行していないことを判定するための閾値である。また、ロボットの振動等に起因する外乱により、進行の判定が困難な場合には、検出された力及びモーメントデータにフィルタをかけたものに基づいて判断することも可能である。

v）ユーザが設定した時間内に、嵌合ワークの被嵌合ワーク内への嵌合深さ（挿入深さ）が指定深さに満たないとき。この場合も、嵌合が進行していないと判断できる。例えば、ステップＳ４の力制御動作のよってワークの姿勢を修正している最中であっても、予め指定した時間内で予め指定した深さ嵌合されない場合は、強制的に探索動作へ移行させることができる。

ステップＳ７において探索動作に移行すべき（嵌合が進行していない）と判断された場合は、ステップＳ８に進んでワーク姿勢の探索動作を行う。図４は、例えば探索動作開始前の嵌合ワークの姿勢が点４０で示されるものであり、かつ図４のグラフの原点Ｏが嵌合に理想的な姿勢であるとする場合に、Ｘ、Ｙ方向回りの２成分にそれぞれ角速度指令を与えて嵌合ワークの姿勢を変化させる例を説明する図である。グラフの横軸及び縦軸はそれぞれ、嵌合ワークのＸ軸及びＹ軸回りの姿勢（角度）を示している。この場合、鋸歯状のグラフ４２における原点Ｏの近傍に相当する姿勢が適正姿勢として判断されることになる。

図５は、図４に示した探索動作の他の具体例を示す図である。図５の例では、Ｘ、Ｙ方向回りの２成分を同時に変化させるのではなく、１成分ずつ変化させる探索動作を行う。具体的には、先ずＹ方向回りの成分を固定してＸ方向回りについての成分を変化させて（グラフ４４、４６）Ｘ方向回りについての最適姿勢を探索し、次にＸ方向回りの成分を固定してＹ方向回りについての成分を変化させる（グラフ４８、５０）。無論、Ｘ、Ｙの順序は逆でもよい。全成分について力制御を行いながら、探索をする成分についてはモーメント指令を加えることで適正姿勢を探索する。

ステップＳ８での探索動作によって適正姿勢が得られたら、再び処理を力制御動作へ移行させる（ステップＳ９）。例えば図５の探索方法を採用した場合、成分毎に以下のvi）〜ix）の何れかの条件で判定を行い、２成分についてその条件を満たしたらステップＳ２に戻る。

vi）嵌合ワークに作用する嵌合方向の力がある閾値以下になったとき。
vii）先ず嵌合方向の力がある閾値以下になったときの嵌合ワーク又はロボットの姿勢（第１の姿勢）を記録する。次に姿勢の変化を続けて、嵌合方向の力がある閾値以上になったときの嵌合ワーク又はロボットの姿勢（第２の姿勢）を記録し、姿勢の変化を中断する。その後第１の姿勢と第２の姿勢の間の姿勢へ動作させる。
viii）嵌合ワークの嵌合方向の速度がある閾値以上になったとき。
ix）先ず嵌合方向の速度がある閾値以上になったときの嵌合ワーク又はロボットの姿勢（第３の姿勢）を記録する。次に姿勢の変化を続けて、嵌合方向の速度がある閾値以下になったときの嵌合ワーク又はロボットの姿勢（第４の姿勢）を記録し、姿勢の変化を中断する。その後第３の姿勢と第４の姿勢の間の姿勢へ動作させる。

上記vi）〜ix）の条件について具体例を挙げて説明する。図６は図９（ｂ）における力の時間変化グラフにおいて、横軸の１２秒から１４秒までを拡大したものである。図６からわかるように、経過時間１２．２秒から１３．０秒までの間は力が抜けており（その前後の時間帯と比べ力が大きく低下しており）、この間はワーク又はワークを把持しているロボットの姿勢が適正（嵌合進行可能な状態）であることが予想される。従って、例えば力が所定の閾値（例えば５Ｎ）を下回ったとき（１２．２秒）に姿勢探索動作を中断させ、１２．２秒での姿勢を適正姿勢としてワーク又はロボットの姿勢を該適正姿勢に維持して、嵌合操作を続行することが考えられる（vi）。或いは、少なくとも力が該閾値を下回ってから（１２．２秒）再び該閾値を上回る（１３．０秒）までの間、姿勢変化を行うとともに各時刻とワークの姿勢との関係を適当な記憶手段に記憶しておき、力が閾値を下回っている間、好ましくは中間（１２．６秒）での姿勢を適正姿勢としてワーク又はロボットの姿勢を該適正姿勢に変更して、嵌合操作を続行することも可能である（vii）。このようにすれば、高速かつ無駄な力をかけることなく、所望の嵌合作業を実行できる。なお図６では力について説明したが速度（viii、ix）についても同様の考え方が適用できる。

なお嵌合が進むにつれて、上述の力が抜ける領域（図５の例では１２．２秒〜１３．０秒）が狭まることが予想される。つまり１回探索動作を行い上記の方法で正しい姿勢を判定し従来の力制御動作へ移行した後であっても、再び嵌合が進行しなくなる可能性がある。その時点でまだ嵌合が完了していない場合には、以下のx）〜xii）の何れかの条件の成立をトリガーとして、再度探索動作へ移行させるかどうか判断することができる。なお下記の閾値や所定の深さ等は、経験的に求めることができる。

x）嵌合方向の力がある閾値以上になったとき
xi）嵌合方向の速度がある閾値以下になったとき
xii）所定の時間経過しても、ワークが所定の深さまで嵌合されないとき

以下の式（１５）及び（１６）は、探索動作を終了させて力制御動作へ移行させるための条件である。式（１５）及び（１６）の少なくとも一方を満たしたら、ワーク又はロボットの姿勢が適正姿勢となったと判定する。

式（１５）及び（１６）において、Ｒ_F、Ｒ_vはいずれも、ユーザが設定する１以下の定数である。本発明では例えば、探索動作を以下のxiii）〜xv）の３通りから選択できる。
xiii）２方向同時での探索動作
上記（６）式のＭ_Xd、ω_Xdのうち少なくとも１つをゼロでない値にし、式（７）のＭ_Yd、ω_Ydのうち少なくとも１つをゼロでない値にすることにより、２方向同時の探索動作を行い、式（１５）及び（１６）の少なくとも一方が満たされたら、力制御動作へ移行する。
xiv）１方向ずつの探索動作
式（７）のＭ_Yd、ω_Ydを両方ともゼロにし、式（６）のＭ_Xd、ω_Xdのうち少なくとも１つをゼロでない値にすることにより、Ｘ方向回りのみの探索動作を行う。式（１５）及び（１６）の少なくとも一方が満たされたら、式（６）のＭ_Xd、ω_Xdを両方ともゼロにし、式（７）のＭ_Yd、ω_Ydのうち少なくとも１つをゼロでない値にすることにより、Ｙ方向回りのみの探索動作を行う。式（１５）及び（１６）の少なくとも一方が満たされたら、力制御動作へ移行する。
xv）１方向ずつの探索動作でより正しい姿勢で探索動作を停止する方法
上式（７）のＭ_Yd、ω_Ydを両方ともゼロにし、式（６）式のＭ_Xd、ω_Xdのうち少なくとも１つをゼロでない値にすることで、Ｘ方向回りのみの探索動作を行う。式（１５）及び（１６）の少なくとも一方を初めて満たしたときの嵌合ワーク又はロボットの姿勢θ₁を記録して探索動作を継続させ、条件を満たさなくなった瞬間の嵌合ワーク又はロボットの姿勢θ₂を記録して探索動作を停止させ、次式（１７）で示される姿勢θに嵌合ワーク又はロボットがなるようにロボットを動作させる。

嵌合ワーク又はロボットが上記姿勢θを呈するようにロボットを動作させた後、同様にＹ軸回りのみの探索動作を行い、式（１５）及び（１６）の条件を初めて満たしたときの嵌合ワーク又はロボットの姿勢θ₁を記録して探索動作を継続させ、条件を満たさなくなった瞬間の嵌合ワーク又はロボットの姿勢θ₂を記録して探索動作を停止させ、式（１７）で示される姿勢θに嵌合ワーク又はロボットがなるようにロボットを動作させた後、力制御動作へ移行する。

本発明によれば、従来の力制御動作と探索動作とを自動的に使い分けて嵌合作業を行うことができるようになり、結果として効率の良い嵌合が実現できる。

１０ 嵌合装置
１１ ロボット
１２ ロボットアーム
１４ 力検出器
１６ 制御装置
１８ テーブル
２０ ハンド
Ｗ１ 被嵌合ワーク
Ｗ２ 嵌合ワーク

Claims (5)

1. 嵌合ワークを把持したロボットにより、該嵌合ワークを定位置に設置された被嵌合ワークに嵌合する嵌合装置において、
   前記嵌合ワークに作用する力を測定する力測定部と、
   前記被嵌合ワークに対する前記嵌合ワークの嵌合が進行しているか否かを判断する嵌合進行判断部と、
   前記嵌合進行判断部により嵌合が進行していないと判断された場合に、前記嵌合ワークの現在の姿勢を変化させ、前記嵌合ワークの姿勢を変化させている間、前記力測定部により得られた嵌合方向の力が所定の閾値を下回ったとき又は前記嵌合ワークの嵌合方向の速度が所定の閾値を上回ったときと、前記力が所定の閾値を上回ったとき又は前記速度が所定の閾値を下回ったときとの間の前記嵌合ワークの姿勢を適正姿勢と判断するワーク姿勢探索部と、
   前記ワーク姿勢探索部により探索された前記嵌合ワークの適正姿勢を用いて、前記嵌合ワークを把持したロボットに嵌合動作を継続させる嵌合動作指示部と、
   を備えることを特徴とする嵌合装置。
2. 前記ワーク姿勢探索部は、各々が前記嵌合ワークの嵌合方向と直交しかつ互いに直交する２方向のうち、少なくとも１つの方向回りについて前記嵌合ワークの姿勢変化を行わせる、請求項１に記載の嵌合装置。
3. 前記ワーク姿勢探索部は、前記嵌合ワークに設定された制御点を中心に前記嵌合ワークの姿勢を所定角度だけ往復変化させ、該往復変化の間に前記力測定部によって得られた嵌合方向の力、又は前記嵌合ワークの嵌合方向の速度に基づいて前記嵌合ワークの適正姿勢を探索する、請求項１又は２に記載の嵌合装置。
4. 前記力測定部は、前記嵌合ワークに作用する力及びモーメントを測定するように構成され、前記嵌合装置は、前記モーメントが小さくなる方向に前記嵌合ワークの姿勢を力制御によって修正する力制御部をさらに有する、請求項１〜３のいずれか１項に記載の嵌合装置。
5. 嵌合ワークを把持したロボットにより、該嵌合ワークを定位置に設置された被嵌合ワークに嵌合する嵌合方法において、
   前記嵌合ワークを前記被嵌合ワークに押付けるステップと、
   前記嵌合ワークに作用する力を測定するステップと、
   前記被嵌合ワークに対する前記嵌合ワークの嵌合が進行しているか否かを判断するステップと、
   前記被嵌合ワークに対する前記嵌合ワークの嵌合が進行していないと判断された場合に、前記嵌合ワークの現在の姿勢を変化させ、前記嵌合ワークの姿勢を変化させている間、前記嵌合ワークに作用する嵌合方向の力が所定の閾値を下回ったとき又は前記嵌合ワークの嵌合方向の速度が所定の閾値を上回ったときと、前記力が所定の閾値を上回ったとき又は前記速度が所定の閾値を下回ったときとの間の前記嵌合ワークの姿勢を適正姿勢と判断するステップと、
   探索された前記嵌合ワークの適正姿勢を用いて、前記嵌合ワークを把持したロボットに嵌合動作を継続させるステップと、
   を備えることを特徴とする嵌合方法。

Images