(57)【要約】      （修正有）【課題】嵌合作業時の噛付きをより確実に短い時間で解消する。【解決手段】嵌合部品を把持する把持手段１２０４と、前記把持手段によってされた嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段１２０７と、を備え、前記嵌合部品を被嵌合部品に嵌合させるロボットにおいて、嵌合途中でかじり付き状態であると判断する間は、挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加する。【選択図】図１２
2008264910.tif 000002
【特許請求の範囲】
【請求項１】
嵌合部品を把持する把持手段と、前記把持手段によって把持された嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段と、を備え、前記嵌合部品を被嵌合部品に嵌合させるロボットにおいて、
嵌合途中で噛付き状態であると判断する間は、
挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加することを特徴とするロボットの制御方法。
【請求項２】
前記挿入動作は、前記力モーメント検出手段で検出された力モーメント信号に基づく力制御によって制御されることを特徴とする請求項１記載のロボットの制御方法。
【請求項３】
前記振動力は、前記力モーメント検出手段の信号に、大きさと方向が周期的に変化する振動成分が付加されることによって発生することを特徴とする請求項２記載のロボットの制御方法。
【請求項４】
前記振動力は、前記力制御の目標力に、大きさと方向が周期的に変化する振動力を与えることによって発生することを特徴とする請求項２記載のロボットの制御方法。
【請求項５】
前記嵌合部品の噛付き状態の発生は、前記嵌合部品の挿入速度が所定の閾値よりも小さい否かで判断され、
噛付き状態が発生していないと判断する場合の閾値は、噛付き状態が発生していると判断する場合の閾値よりも大きいことを特徴とする請求項１乃至４いずれかに記載のロボットの制御方法。
【請求項６】
前記振動力の振動方向が、前記嵌合部品の挿入方向に対して回転することを特徴とする請求項１乃至５いずれかに記載のロボットの制御方法。
【請求項７】
嵌合部品を把持する把持手段と、前記把持手段によって把持された嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段と、を備えたロボットを制御して、前記嵌合部品を被嵌合部品に嵌合させるロボットの制御装置において、
嵌合途中で噛付き状態であると判断する間は、
挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加する手段を備えたことを特徴とするロボット制御装置。
【請求項８】
前記挿入動作は、前記力モーメント検出手段で検出された力モーメント信号に基づく力制御によって制御されることを特徴とする請求項７記載のロボット制御装置。
【請求項９】
前記振動力は、前記力モーメント検出手段の信号に、大きさと方向が周期的に変化する振動成分が付加されることによって発生することを特徴とする請求項８記載のロボット制御装置。
【請求項１０】
前記振動力は、前記力制御の目標力に、大きさと方向が周期的に変化する振動力を与えることによって発生することを特徴とする請求項８記載のロボット制御装置。
【請求項１１】
前記嵌合部品の噛付き状態の発生は、前記嵌合部品の挿入速度が所定の閾値よりも小さい否かで判断され、噛付き状態が発生していないと判断する場合の閾値は、噛付き状態が発生していると判断する場合の閾値よりも大きいことを特徴とする請求項７乃至１０いずれかに記載のロボット制御装置。
【請求項１２】
前記振動力の振動方向が、前記嵌合部品の挿入方向に対して回転することを特徴とする請求項７乃至１１いずれかに記載のロボット制御装置。
【請求項１３】
嵌合部品を把持する把持手段と前記把持手段によって把持された嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段とから少なくとも構成されるロボットと、
前記ロボットを制御するロボットの制御装置と、を備え、
前記嵌合部品を被嵌合部品に嵌合させるロボットの制御システムにおいて、
嵌合途中で噛付き状態であると判断する間は、
挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加する手段を備えたことを特徴とするロボットの制御システム。
【請求項１４】
前記挿入動作は、前記力モーメント検出手段で検出された力モーメント信号に基づく力制御によって制御されることを特徴とする請求項１３記載のロボットの制御システム。
【請求項１５】
前記振動力は、前記力モーメント検出手段の信号に、大きさと方向が周期的に変化する振動成分が付加されることによって発生することを特徴とする請求項１４記載のロボットの制御システム。
【請求項１６】
前記振動力は、前記力制御の目標力に、大きさと方向が周期的に変化する振動力を与えることによって発生することを特徴とする請求項１４記載のロボットの制御システム。
【請求項１７】
前記嵌合部品の噛付き状態の発生は、前記嵌合部品の挿入速度が所定の閾値よりも小さい否かで判断され、
噛付き状態が発生していないと判断する場合の閾値は、噛付き状態が発生していると判断する場合の閾値よりも大きいことを特徴とする請求項１３乃至１６いずれかに記載のロボットの制御システム。
【請求項１８】
前記振動力の振動方向が、前記嵌合部品の挿入方向に対して回転することを特徴とする請求項１３乃至１７いずれかに記載のロボットの制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、産業用ロボットを用いて嵌合作業するロボットの制御方法、装置およびシステムに関するもので、特に挿入途中に噛付きを生じたときにこれを解消する機能に関する。
【背景技術】
【０００２】
産業用ロボットは、ハンドリング、溶接、塗装など多岐に渡る用途に普及しているが、部品と部品の接触を伴う組立用途についてはロボットによる作業遂行自体が難しく、他用途比べてあまり普及していない。例えば、嵌合作業では、嵌合途中に部品同士が噛付きやすいため、作業成功率が低くなってしまう。
図１２はロボットによる嵌合作業の一般的なシステム構成図を示している。
図１２において、１２０１はロボット、１２０２ａ〜１２０３ｄはロボット１２０１の各関節を駆動するアクチュエータである。１２０３は、アクチュエータ１２０２ａ〜１２０２ｄの運動を制御してロボット１２０１を動作させる制御装置である。１２０４はロボット１２０１の先端に設けられたグリッパ（把持手段）である。１２０５ａはグリッパ１２０４で把持する嵌合部品である。１２０５ｂは嵌合部品１２０５ｂが嵌め合わされる穴が空いた被嵌合部品である。
【０００３】
図１３は、嵌合作業の開始状態と終了状態（状態遷移）を示している。図１３（ａ）に示すように、グリッパ１２０４で円柱形状の嵌合部品１２０５ａを把持した状態で、嵌合部品１２０５ａを被嵌合部品１２０５ｂの穴方向（図ではZ負方向）に移動させ、図１３（ｂ）に示すように穴に接触させて押し付ける。図１３（ｃ）は、嵌合が成功した状態を示しており、嵌合部品１２０５ａが被嵌合部品１２０５ｂの穴底まで挿入されている。図１３（ｄ）は、嵌合が失敗した例を示しており、嵌合部品１２０５ａのエッジが被嵌合部品１２０５ｂの穴に引っかかって噛付きが生じている。
嵌合部品と被嵌合部品の隙間が小さい場合、嵌合部品が微小に傾いているだけで、上述したような噛み付きが発生し、嵌め合い作業の遂行は困難である。
部品と部品の接触を伴う作業の場合、部品が受ける力およびモーメントを検出する力モーメントセンサをロボットのエンドエフェクタに取り付け、検出した力・モーメントに応じてロボットの運動を制御（以降、力制御と呼ぶ）するのが一般的（公知の技術）である。
【０００４】
力制御の実現方法を図１２に基づいて簡単に説明する。
１２０６ａ〜１２０６ｄは、アクチュエータ１２０２ａ〜１２０２ｄの回転角度を検出する回転角センサ（エンコーダ）である。１２０７は、ロボット１２０１のエンドエフェクタ（グリッパ１２０４）に取り付けられた力モーメントセンサである。１２０８は、エンコーダ１２０６ａ〜１２０６ｄからの信号を取得するエンコーダ信号処理部である。１２０９は、力モーメントセンサ１２０７からの信号をもとに嵌合部品１２０５ａが受ける力およびモーメントを計算する力モーメント信号処理部である。１２１０はエンコーダ信号処理部１２０８および力モーメント信号処理部１２０９からの信号をもとにフィードバック制御系を構成して、アクチュエータ１２０２ａ〜１２０２ｄへの駆動力を調節する動作制御部である。１２１１は、動作制御部１２１０に対して位置姿勢および力モーメントに関する目標指令を与える動作指令部である。１２１２は、動作制御部１２１０の出力をもとにアクチュエータ１２０２ａ〜１２０２ｄへ駆動力（電力）を供給するアクチュエータ駆動アンプ部である。また、１２１３は、動作指令部１２１１に与える動作プログラムを作成・編集（教示）する可搬式教示操作盤である。
動作制御部１２１０で実現する力制御としては、嵌合部品１２０５ａが受ける反力に対して仮想的な柔軟性（バネ特性）を実現するインピーダンス制御やコンプライアンス制御が一般的である（公知の技術）。嵌合部品１２０５ａに仮想的に実現したバネ特性により、ロボットの位置決め誤差や嵌合部品１２０５ｂの位置誤差を許容して嵌合作業が可能になるが、力制御のみでは噛付きを解消することはできない。
【０００５】
挿入途中の噛付きを解消する技術が特許文献１および特許文献２に開示されている。
特許文献１では、ハンドの根元部分に切り込みと振動子（圧電素子など）を設け、振動子によってハンド先端に振動を付加することによって噛み付きを解消している。
特許文献２では、つまり（噛付き）が検出されたときに嵌合（挿入）を中止し、嵌合（挿入）方向と交差する方向に打撃力を加えることによって、噛付きを解消して嵌合（挿入）を再開している。また、噛付き発生時の姿勢変化やモーメント変化から、嵌合部品の被嵌合部品に対する傾きを検出し、傾き方向に打撃力を加えている。
【特許文献１】特開平４−３２２９９４号公報（第３−４頁、図１）
【特許文献２】特許第３８１８９８６号公報（第４−１２頁、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００６】
特許文献１では、ハンド根元に付加した振動子で加振できる重量は軽いので、嵌合部品が重い場合には適用できないという問題がある。
一方、特許文献２では、ロボットのアクチュエータを利用して嵌合部品に打撃力を加えるので重い嵌合部品に対しても適用できるが、噛付きが検出されたときに嵌合（挿入）を中止して打撃力を加えてから嵌合（挿入）を再開しているので、噛付き解消効果が低いという問題がある。さらに、打撃力を加えてから嵌合（挿入）を再開する基準が示されておらず、打撃力が不十分の場合には噛付きが解消されないまま同じ処理を繰り返してしまうため、タクトタイムが長くなるという問題もある。また、噛付き発生時の姿勢変化やモーメント変化が微小で検出不可能な場合には、打撃力を加える方向を決定できないという問題もある。
本発明はこのような状況を鑑みてなされたものであり、噛付き解消効果が高く、タクトタイムが短く、噛付き発生時の姿勢変化やモーメント変化が微小な場合でも適用できる嵌合作業のロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記問題を解決するため、本発明はつぎのようにしたのである。
請求項１に記載の発明は、嵌合部品を把持する把持手段と、前記把持手段によって把持された嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段と、を備え、前記嵌合部品を被嵌合部品に嵌合させるロボットにおいて、嵌合途中で噛付き状態であると判断する間は、挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加することを特徴とするものである。
請求項２に記載の発明は、前記挿入動作は、前記力モーメント検出手段で検出された力モーメント信号に基づく力制御によって制御されることを特徴とするものである。
請求項３に記載の発明は、前記振動力は、前記力モーメント検出手段の信号に、大きさと方向が周期的に変化する振動成分が付加されることによって発生することを特徴とするものである。
請求項４に記載の発明は、前記振動力は、前記力制御の目標力に、大きさと方向が周期的に変化する振動力を与えることによって発生することを特徴とするものである。
請求項５に記載の発明は、前記嵌合部品の噛付き状態の発生は、前記嵌合部品の挿入速度が所定の閾値よりも小さい否かで判断され、噛付き状態が発生していないと判断する場合の閾値は、噛付き状態が発生していると判断する場合の閾値よりも大きいことを特徴とするものである。
請求項６に記載の発明は、前記振動力の振動方向が、前記嵌合部品の挿入方向に対して回転することを特徴とするものである。
請求項７に記載の発明は、嵌合部品を把持する把持手段と、前記把持手段によって把持された嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段と、を備えたロボットを制御して、前記嵌合部品を被嵌合部品に嵌合させるロボットの制御装置において、嵌合途中で噛付き状態であると判断する間は、挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加する手段を備えたことを特徴とするものである。
請求項８に記載の発明は、前記挿入動作は、前記力モーメント検出手段で検出された力モーメント信号に基づく力制御によって制御されることを特徴とするものである。
請求項９に記載の発明は、前記振動力は、前記力モーメント検出手段の信号に、大きさと方向が周期的に変化する振動成分が付加されることによって発生することを特徴とするものである。
請求項１０に記載の発明は、前記振動力は、前記力制御の目標力に、大きさと方向が周期的に変化する振動力を与えることによって発生することを特徴とするものである。
請求項１１に記載の発明は、前記嵌合部品の噛付き状態の発生は、前記嵌合部品の挿入速度が所定の閾値よりも小さい否かで判断され、噛付き状態が発生していないと判断する場合の閾値は、噛付き状態が発生していると判断する場合の閾値よりも大きいことを特徴とするものである。
請求項１２に記載の発明は、前記振動力の振動方向が、前記嵌合部品の挿入方向に対して回転することを特徴とするものである。
請求項１３に記載の発明は、嵌合部品を把持する把持手段と前記把持手段によって把持された嵌合部品に加わる力およびモーメントを検出する力モーメント検出手段とから少なくとも構成されるロボットと、前記ロボットを制御するロボットの制御装置と、を備え、前記嵌合部品を被嵌合部品に嵌合させるロボットの制御システムにおいて、嵌合途中で噛付き状態であると判断する間は、挿入動作を継続するとともに、大きさと方向が周期的に変化する振動力を、前記把持手段を介して前記嵌合部品に付加する手段を備えたことを特徴とするものである。
請求項１４に記載の発明は、前記挿入動作は、前記力モーメント検出手段で検出された力モーメント信号に基づく力制御によって制御されることを特徴とするものである。
請求項１５に記載の発明は、前記振動力は、前記力モーメント検出手段の信号に、大きさと方向が周期的に変化する振動成分が付加されることによって発生することを特徴とするものである。
請求項１６に記載の発明は、前記振動力は、前記力制御の目標力に、大きさと方向が周期的に変化する振動力を与えることによって発生することを特徴とするものである。
請求項１７に記載の発明は、前記嵌合部品の噛付き状態の発生は、前記嵌合部品の挿入速度が所定の閾値よりも小さい否かで判断され、噛付き状態が発生していないと判断する場合の閾値は、噛付き状態が発生していると判断する場合の閾値よりも大きいことを特徴とするものである。
請求項１８に記載の発明は、前記振動力の振動方向が、前記嵌合部品の挿入方向に対して回転することを特徴とするものである。
【発明の効果】
【０００８】
請求項１から４に記載の発明によると、噛付きが検出されている間、挿入動作を中断せずに嵌合部品に周期的に大きさと方向が変化する振動力を加えるので、噛付き解消効果が高くなるという効果がある。
また、請求項５に記載の発明によると、噛付き発生を検出する閾値と、噛付き解消を検出する閾値を異ならせているので、タクトタイムが短くなるという効果がある。
また、請求項６に記載の発明によると、嵌合部品に加える振動力の方向を時間とともに回転させるので、噛付き発生時の姿勢変化やモーメント変化が微小で検出不可能な場合にも適用できるという効果がある。
【発明を実施するための最良の形態】
【０００９】
以下、本発明の方法の具体的実施例について、図１２および図１３に示した円柱形状部品の嵌合作業を例に説明する。
【実施例１】
【００１０】
図１は本発明における嵌合作業のロボット制御装置の構成図を示している。
図１において、１０１は可搬式教示操作盤１２１３で作成された動作プログラム（教示データ）を示している。動作プログラム１０１は動作指令部１２１１に送られ、インタプリタ１０２で解釈・実行される。モーション指令生成部１０３はインタプリタ１０２から発行された動作コマンドを受けて、ロボットの位置指令を生成する。モーション指令生成部１０３で生成された位置指令は、動作制御部１２１０に送られ、サーボループ処理部１０４にて各軸アクチュエータが追従制御される。
１０５は力制御部であり、力モーメント検出手段１２０７で検出された力およびモーメントのフィードバック信号をもとに位置の修正指令を計算する。力制御部１０５で計算された位置修正指令はモーション指令生成部１０３から送られてくる位置指令と加算されてサーボループ処理部１０４に与えられる。力制御方式としては、例えばインピーダンス制御（公知の技術）が利用しやすい。
【００１１】
図１４にインピーダンス制御のブロック図の例を示す。
図１４において、Frefは力モーメント指令（力モーメント目標値）、Ffbは力モーメントフィードバック値である。θrefはモーション指令生成部１０３から送られてくる位置指令（関節座標系）、δθはインピーダンス制御部１０５が計算する位置修正量である。インピーダンス制御部１０５では、まず、FrefとFfbをもとに、次式にしたがって直交座標系における位置修正量δPが計算される（1401）。
δP ＝（Ｍｓ2＋Ｄｓ＋Ｋ）-1（Fref−Ffb）
ここで、Ｍ，Ｄ，Ｋはそれぞれ、慣性マトリクス、粘性係数マトリクス、剛性マトリクス（バネ定数）である。通常、これらは対角行列として、各軸方向独立なインピーダンス特性を設定する。また、ｓはラプラス演算子であり、時間に関する一階微分に相当する。
直交座標系における位置修正量δPは、ヤコビ行列Ｊ(θ)を用いて次式により関節座標系の位置修正量δθに分解される（1402）。
δθ＝Ｊ(θ)-1 δP
このδθをθrefに足し合わせた位置指令θref’をサーボループ処理部１０４に与えることによって、外力やモーメントに対して、Ｍ，Ｄ，Ｋで指定された特性を保ちながらロボットが動作する。例えば、Ｋにより外力に対してロボットがバネのように動作し、その際、ＭおよびＤを小さくすることによって軽くスムーズに動作する。
【００１２】
動作プログラム１０１の中で、「ＭＯＶ  Ｐ１」は嵌合を開始する位置へロボット１２０１のグリッパ（把持手段）１２０４を移動させるコマンドである。「ＴＳＵＫＩＡＴＥ」および「ＩＮＳＥＲＴ」が嵌合作業のコマンドである。嵌合作業のコマンド「ＴＳＵＫＩＡＴＥ」および「ＩＮＳＥＲＴ」は、インタプリタ１０２から動作制御部１２１０に送信され、コマンド受付部１０６にて受付処理される。コマンド受付部１０６はコマンドに応じたルーチン（関数）を選択し、そのルーチンは力制御（インピーダンス制御）部１０５から制御周期毎に呼び出しされて実行される。「ＴＳＵＫＩＡＴＥ」コマンドの場合は、突き当て動作を実行するためのルーチンである突き当て動作実行手段１０７が呼び出され、「ＩＮＳＥＲＴ」コマンドの場合は、挿入動作を実行するためのルーチンである挿入動作実行手段１０８が呼び出される。これら嵌合作業のコマンドの実行結果は、コマンドアンサー部１０９を介してインタプリタ１０２に返信される。また、嵌合作業のコマンド内の処理は、位置や力モーメントのセンサ信号をもとに作業状態を検知・判定する処理部と、インピーダンス制御部のパラメータや力指令値を変更する処理部に大別される。挿入動作実行手段１０８の場合は、これらの処理の一例として、噛付きの発生を検出する噛付き判断手段１１０、発生した噛付きを解消する動作を実行するための噛付き解消動作実行手段１１１が内部に含まれる。
【００１３】
つぎに、図１に示した嵌合作業のロボット制御装置で、動作プログラム１０１を実行することにより、嵌合作業がどのように進行していくかを具体的に説明する。
動作プログラム１０１の「ＭＯＶ  Ｐ１」を実行することにより、ロボットは把持した嵌合部品を、図１３（ａ）のように被嵌合部品の上方に移動させる。このときはまだ嵌合部品と被嵌合部品を接触させない。「ＴＳＵＫＩＡＴＥ」コマンドが実行されると、突き当て動作実行手段１０７がインピーダンス制御部１０５から制御周期毎に実行され、嵌合部品が被嵌合部品に突き当てられる。
【００１４】
図２は、突き当て動作実行手段１０７の処理のフローチャートを示している。
図２において、S200は周期処理の開始、S218は周期処理の終了を示している。S201では、インピーダンス制御部１０５のパラメータの中で嵌合方向のバネ定数をゼロに設定する。S202は、突き当て動作実行手段１０７の内部で保持されている接触フラグが１（接触状態）かゼロ（非接触状態）かを判断する。接触フラグがゼロの場合は、S203にて、嵌合方向の力目標値をFref=Vapp×Dと設定する。ここで、Vappは嵌合部品を被嵌合部品に接近させる速度である。Dはインピーダンス制御部１０５の嵌合方向の粘性係数である。嵌合方向のバネ定数をゼロにして、嵌合方向の力目標値をこのように設定することにより、嵌合部品は定常速度Vappで被嵌合部品に向かって接近する。S204では、力モーメント検出手段の信号をもとに力フィードバックの絶対値を計算する。S205は、S204で計算した力フィードバックの絶対値をもとに接触を検出する。S205では、力フィードバックの絶対値が予め設定した接触検出閾値以下の場合は非接触状態と判断し、何もせずにつぎのステップS206に移る。S206では、突き当て手段１０７が内部で保持しているリターンコード（RC）をゼロにし、S207でリターンコードを出力して終了する。ここで、リターンコードがゼロとは、突き当て動作が実行中であることを意味している。
S205において、力フィードバックの絶対値が接触検出閾値より大きい場合は、接触状態と判断し、接触フラグを１にする。また、グリッパ制御点（TCP）の接触位置を記憶する。接触フラグが１に変更されると、次周期以降の呼び出し時にS202にて接触フラグが１と判断されるので、S208にて嵌合方向の力目標値がFref=Fcmd1と設定される。Fcmd1は突き当て時にワーク（嵌合部品と非嵌合部品）に加えるべき力である。嵌合部品が非嵌合部品の穴の面取り内部に接触すれば、インピーダンス制御状態で力目標値を加えることにより、嵌合部品は非嵌合部品の穴に自然に嵌り込む。S209では、突き当て手段１０７が内部で保持している終了フラグがゼロ（実行中）か１（実行終了）かを判断する。接触検出直後は実行中であるので、終了フラグはゼロであり、S210に処理が移る。S210では、突き当て手段１０７が内部で保持しているタイマカウンタtm_cnt1がインクリメントされる。S211では、接触検出後のグリッパ制御点の移動量絶対値を挿入量として計算する。S212では、S211で計算した挿入量が予め設定した挿入量閾値１と比較される。挿入量が挿入量閾値１以下の場合は、S213にてタイマカウンタtm_cnt1がタイマ閾値１と比較される。タイマカウンタtm_cnt1がタイマ閾値１以下の場合はS206に移る（実行中）。タイマカウンタtm_cnt1がタイマ閾値１より大きい場合は、挿入量が不足したまま一定時間が経過したことを意味するので、S214にてリターンコードを１（失敗）にし、S215で終了フラグを１にし、S207でリターンコードを出力して終了する。
S212にて挿入量が挿入量閾値１より大きい場合は、嵌合部品が被嵌合部品の穴に嵌り込んだ（突き当てに成功した）と判断し、S216にてリターンコードを２（成功）にし、S217で終了フラグを１にし、S207でリターンコードを出力して終了する。「ＴＳＵＫＩＡＴＥ」コマンドが成功（RC=2）として終了すると、嵌合部品と被嵌合部品は図１３（ｂ）の状態になる。インタプリタ１０２は次のコマンド「ＩＮＳＥＲＴ」を実行し、インピーダンス制御部１０５で挿入動作実行手段１０８が呼び出される。
【００１５】
図３は、挿入動作実行手段１０８の処理のフローチャートを示している。
図３において、S300は周期処理の開始、S313は周期処理の終了を示している。S301では、インピーダンス制御部１０５のパラメータの中で嵌合方向のバネ定数をゼロに設定する。S302では、挿入動作実行手段１０８の内部で保持されている終了フラグがゼロ（実行中）か１（実行終了）かを判断する。終了フラグがゼロの場合は、S303にて挿入方向の力目標値をFref=Fcmd2と設定する。これにより、嵌合部品が被嵌合部品の穴底に向かって移動し始める。終了フラグが１の場合は、S304にて挿入方向の力目標値をゼロにする。つぎにS305にて、噛付きフラグ（挿入動作実行手段１０８の内部で保持されている）がゼロ（噛付きなし）か１（噛付きあり）かを判断する。噛付きフラグが１の場合はS306で噛付き解消動作を実行開始してS307に移る。噛付き解消動作については後述する。噛付きフラグがゼロの場合はS306を実行せずにS307に移る。S307では、接触検知後の挿入量と挿入速度（ともに絶対値）が計算される。動作が振動的になる場合があるので、挿入速度からローパスフィルタで高次振動成分を除去してもよい。S308は、S307で計算した挿入量と挿入速度をもとに噛付きの発生を検知し、噛付きフラグを１に設定する（噛付き発生が検知されない場合はなにもしない）。S309は挿入動作が正常終了したかどうかを検出する処理で、正常終了を検知した場合は終了フラグを１に、リターンコードを２に設定する。S310は挿入動作が異常終了したかどうかを検出する処理で、異常終了を検知した場合は終了フラグを１に、リターンコードを１に設定する。S311はタイムアウトを検出する処理で、正常終了も異常終了も検出されないまま長時間経過した場合にタイムアウトとし、強制的に終了フラグを１にする。S312でリターンコードを出力して周期処理を終了する。
以上が、挿入動作実行手段１０８の基本的な動作フローである。噛付きが発生しても挿入動作を中断することなく、噛み付き解消動作を実行するところが従来にない特長である。
【００１６】
図４は、図３における噛付き検出処理S308および正常終了検出処理S309の詳細フローチャートを示している。
S401からS407が噛付き検出処理S308に、S408からS412が正常終了検出処理S309に該当する。噛付き検出処理では、挿入が一定時間進まず、かつ、挿入量が不十分の場合に噛付き検出と判断される。まず、S401では、S307で計算された挿入速度（絶対値）が速度閾値１未満であるかどうか判断される。挿入速度が速度閾値１未満の場合はタイマカウンタtm_cnt2をインクリメントし（S402）、挿入速度が速度閾値１以上の場合はタイマカウンタtm_cnt2をゼロリセットする（S403）。S404ではタイマカウンタtm_cnt2がカウンタ閾値２を超えたかどうか判断し、S405では挿入量が挿入量閾値２未満かどうか判断する。タイマカウンタtm_cnt2がカウンタ閾値２を超え、かつ、挿入量が挿入量閾値２未満の場合は噛付きが発生したと判断し、噛付きフラグを１に設定する（S406）。S404とS405の少なくとも一方の条件が満たされない場合、噛付きは発生していないと判断して噛付きフラグをゼロに設定する（S407）。
正常終了検出処理では、挿入が一定時間進まず、かつ、挿入量が十分の場合に正常終了と判断される。S408ではタイマカウンタtm_cnt2がカウンタ閾値２’を超えたかどうか判断し、S409では挿入量が挿入量閾値２以上かどうか判断する。タイマカウンタtm_cnt2がカウンタ閾値２’を超え、かつ、挿入量が挿入量閾値２以上の場合は挿入が正常に終了したと判断し、噛付きフラグをゼロに設定（S410）し、終了フラグを１に設定（S411）し、リターンコードを２に設定（S412）して終了する。S408とS409の少なくとも一方の条件が満たされない場合、正常終了していないと判断し、S410からS412の処理は実行せずに終了する。ここで、カウンタ閾値２’はカウンタ閾値２と異なる値を設定できるものとする（同じ値に設定しても構わない）。
【００１７】
図５は、図３における異常終了検出処理S310の詳細フローチャートを示している。
S501において、噛付きフラグが１（噛付き発生）かどうか判断される。噛付きフラグが１の場合はタイマカウンタtm_cnt3をインクリメントする（S502）。噛付きフラグがゼロの場合は、タイマカウンタtm_cnt3はゼロリセットされる（S503）。S504ではタイマカウンタtm_cnt3がカウンタ閾値３を超えたかどうか判断され、S505では挿入量が挿入量閾値３未満か判断される。タイマカウンタtm_cnt3がカウンタ閾値３を超え、かつ、挿入量が挿入量閾値３未満の場合は、噛付き解消動作を一定時間実行しても挿入が進展しなかったことを意味するので挿入失敗と判断し、終了フラグを１に設定（S506）し、リターンコードを１に設定（S507）して終了する。S504とS505の少なくとも１つの条件が満足されない場合は、S506とS507を実行せずに終了する。
【００１８】
図６は、図３におけるタイムアウト検出処理S311の詳細フローチャートを示している。タイムアウト検出処理S311は、挿入速度が振動的になって図４に示した正常終了検出が働かない場合に機能する。
S601では終了フラグがゼロかどうか判断する。終了フラグがゼロの場合はタイマカウンタtm_cnt4をインクリメントする（S602）。終了フラグが１の場合は何もせずに終了する。S603ではタイマカウンタtm_cnt4がカウンタ閾値４未満かどうか判断される。タイマカウンタtm_cnt4がカウンタ閾値４未満の場合は、リターンコードをセロに設定（S604）して終了する。タイマカウンタtm_cnt4がカウンタ閾値４以上の場合は、終了フラグが１に設定（S605）され、S606にて挿入量が挿入量閾値２未満かどうか判断される。挿入量が挿入量閾値２未満の場合は、リターンコードを１に設定（S607）して終了する。挿入量が挿入量閾値２以上の場合はリターンコードを２に設定（S608）して終了する。
【００１９】
つぎに、噛付き解消動作（図３のS306）の詳細を説明する。ここでは嵌合（挿入）方向がZ軸方向の場合について噛付き解消動作を説明する。
図１０は、噛付き解消動作の原理を模式的に示したものである。図１０に示すように、噛付きフラグが１である間、XY平面内でグリッパが把持した嵌合部品に振動力Fvibを与える（周期的に外力を発生させる）。このとき、図３に示したように挿入方向に力目標値Fcmd2を加えたままで振動力Fvibを印加する。インピーダンス制御部１０５において、力モーメント検出手段から得られる力フィードバック信号のXY成分に擬似的な外乱信号としてFvibを印加することによって、ロボットのアクチュエータにより嵌合部品に振動を発生させることができる。また、インピーダンス制御部１０５の力目標値FrefのXY成分にFvibを入力しても同様の振動を発生させることができる。なお、ロボットの駆動時の静止摩擦を補償するため、ロボットの各アクチュエータ（モータ）のトルク指令にディザー信号を付加する方法（例えば、特開平8-286759、特開平9-33369など）が知られているが、本発明とは目的も構成方法も異なることを付記しておく。
従来技術で説明した特許文献２のように、噛付きが検出されたときに挿入動作を中断して衝撃力を印加する方法では、挿入動作を再開したときに再び噛付きが検出される可能性が高く、噛付き解消効果が低い。噛付きは、スティックスリップを繰り返しながら静止摩擦状態から動摩擦状態にしだいに遷移することによって解消されるため、挿入方向に目標力を加えつつ振動力（周期的な外力）を加えた方が噛み付き解消効果が高い。さらに、噛付きが解消されたかどうかは挿入方向に力を印加して見なければ分からないので、噛付き解消動作中も挿入動作を継続した方が効率がよい。
以上説明したように、挿入方向に力目標値を加えたままで（挿入を停止させずに）嵌合部品に振動力を印加することによって、噛付き解消効果が向上する。
【実施例２】
【００２０】
図７は、本発明の第２実施例における挿入動作実行手段１０８のフローチャートを示している。
図７において、噛付き解消検出処理S701が追加されたことが、実施例１で説明した図３のフローチャートとは異なる点である。それに伴い、噛付き発生検出処理S308の詳細（図４）も若干異なる。
図８は第２実施例における噛付き発生検出処理S308および正常終了検出処理S309の詳細フローチャートを示している。図８において、図４のS407（噛付きフラグをゼロに設定）の処理がないところが第１実施例とは異なる点である（それ以外は同じ）。すなわち、噛付き解消検出処理S701を新たに追加したので、噛付き発生検出処理内では噛付きが解消したかどうかは判断しない。
図９は、噛付き解消検出処理S701の詳細フローチャートを示している。
S901では、S307で計算された挿入速度（絶対値）が速度閾値２を超えたかどうか判断する。挿入速度が速度閾値２を超えた場合はタイマカウンタtm_cnt5をインクリメント（S902）する。挿入速度が速度閾値２以下の場合はタイマカウンタtm_cnt5をゼロリセット（S903）する。S904ではタイマカウンタtm_cnt5がカウンタ閾値５を超えたかどうか判断する。S905では噛付きフラグが１かどうか判断する。タイマカウンタtm_cnt5がカウンタ閾値５を超えており、かつ、噛付きフラグが１の場合のみ噛付きフラグをゼロに設定（S906）して終了する。S904とS905の少なくとも１つの条件が満足されない場合は何もせずに終了する。
ここで、速度閾値２は、噛付き発生検出処理（図４および図８）で使用した速度閾値１よりも大きな値とする（プログラム上は同じか小さい値にしても問題なく動作する）。このように第２実施例では、噛付きを検出するための速度閾値よりも大きな速度が一定時間持続されたことをもって噛付きが解消されたと判断する。
前述したように、噛付きはスティックスリップを繰り返しながら静止摩擦状態から動摩擦状態にしだいに遷移することによって解消されるものであり、ヒステリシスを有する非可逆的なものである。ひとつの速度閾値で噛付き発生と噛付き解消の両方を検出しようとすると、ほとんど挿入が進展せずに噛付き解消動作の開始と停止を繰り返し、非常に作業時間がかかってしまう（タクトタイムが長くなる）。したがって、噛付き発生検出と噛付き解消検出の速度閾値を別々に設定して別々に判断した方が理に適っており、タクトタイムが短くなるという効果がある。
【実施例３】
【００２１】
図１１は、本発明の第３実施例における噛付き解消処理の原理を模式的に示したものである。
図１１に示すように、第３実施例では、挿入方向（力目標値Fcmd2）とは垂直な平面内でインピーダンス制御部１０５に印加する振動力Fvibを時間とともに回転させる。
従来技術の特許文献２では、噛付き発生時の姿勢変化やモーメント変化から嵌合部品の傾き具合を判断して衝撃力を加える方向を決定しているが、突き当て動作から挿入動作を実行したときにほとんど姿勢変化が検出されない場合が多い（したがってモーメント変化も検出されない）ので、実用上は適用が難しい。
嵌合部品の傾き具合が分からずに適当な方向に振動力を加えたのでは一見効率が悪いように思われるが、振動力Fvibを時間とともに回転させることで挿入方向周りの加速度が発生するので高い噛付き解消効果が得られる。また、振動力Fvibを回転させながら挿入速度を監視し、挿入速度の時間変化（すなわち挿入加速度）が一番大きくなる方向を探索してもよい。
以上説明したように、嵌合部品に加える振動力を時間とともに回転させるので、噛付き発生時の姿勢変化やモーメント変化が微小で検出不能な場合でも、高い噛付き解消効果が得られるという効果がある。
【産業上の利用可能性】
【００２２】
本発明は、産業用ロボットによる組立て作業に広く適用することができる。
【図面の簡単な説明】
【００２３】
【図１】本発明における嵌合作業のロボット制御装置の構成図
【図２】突き当て動作実行手段の処理のフローチャート
【図３】第１実施例における挿入動作実行手段の処理のフローチャート
【図４】第１実施例における噛付き発生検出・正常終了検出処理のフローチャート
【図５】異常終了検出処理のフローチャート
【図６】タイムアウト検出処理のフローチャート
【図７】第２実施例における挿入動作実行手段の処理のフローチャート
【図８】第２実施例における噛付き発生検出・正常終了検出処理のフローチャート
【図９】噛付き解消検出処理のフローチャート
【図１０】第１および第２実施例における噛付き解消動作の原理を示す模式図
【図１１】第３実施例における噛付き解消動作の原理を示す模式図
【図１２】一般的な嵌合作業のロボット制御装置の構成図
【図１３】嵌合作業の状態遷移を説明する図
【図１４】インピーダンス制御のブロック図
【符号の説明】
【００２４】
１０１  動作プログラム
１０２  インタプリタ部
１０３  モーション指令生成部
１０４  サーボループ処理部
１０５  インピーダンス制御部
１０６  コマンド受付部
１０７  突き当て動作実行手段
１０８  挿入動作実行手段
１０９  コマンドアンサー部
１１０  噛付き判断手段
１１１  噛付き解消動作実行手段
１２０１    ロボット
１２０２ａ〜１２０２ｄ  アクチュエータ
１２０３    制御装置
１２０４    グリッパ（把持手段）
１２０５ａ  嵌合部品
１２０５ｂ  被嵌合部品
１２０６ａ〜１２０６ｄ  エンコーダ
１２０７    力モーメントセンサ
１２０８    エンコーダ信号処理部
１２０９    力モーメント信号処理部
１２１０    動作制御部
１２１１    動作指令部
１２１２    アクチュエータ駆動アンプ部
１２１３    可搬式教示操作盤
【図１】
2008264910.tif 000003
【図２】
2008264910.tif 000004
【図３】
2008264910.tif 000005
【図４】
2008264910.tif 000006
【図５】
2008264910.tif 000007
【図６】
2008264910.tif 000008
【図７】
2008264910.tif 000009
【図８】
2008264910.tif 000010
【図９】
2008264910.tif 000011
【図１０】
2008264910.tif 000012
【図１１】
2008264910.tif 000013
【図１２】
2008264910.tif 000014
【図１３】
2008264910.tif 000015
【図１４】
2008264910.tif 000016