JP2005028532A

JP2005028532A - 嵌合装置

Info

Publication number: JP2005028532A
Application number: JP2003272046A
Authority: JP
Inventors: Tetsuro Kato; 哲朗加藤; Takayuki Sato; 貴之佐藤
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 2003-07-08
Filing date: 2003-07-08
Publication date: 2005-02-03
Anticipated expiration: 2023-07-08
Also published as: JP3818986B2

Abstract

【課題】ロボットを用いた嵌合作業中につまりが発生した際の適正な対処。
【解決手段】嵌合部品４をハンドで把持したロボットは、アプローチ開始位置から被嵌合部品５に接近し、両部品が接触すると力制御による嵌合動作を開始する。力センサ等で嵌合途中でのつまりが検出されると、つまり解消動作が実行される。力制御に必要な力の検出には、力センサ、トルクセンサ、電流値からのトルク推定などを利用する。つまり解消後は、嵌合動作を再開するか、嵌合部品４の引き抜きを行なう。つまり解消動作は、ロボットを用いて打撃力Ｓ１、Ｓ２を与える動作である。力センサ等を利用して、つまり状態にある両部品の２つの接触点接触点を結ぶ直線を、挿入方向と垂直な平面へ射影した方向を求め、その方向に沿った打撃力を加えることも出来る。打撃力の大きさを力制御で制御することもできる。
【選択図】図６

Description

本発明は、産業用ロボット（以下、ロボットを用いて嵌合部品を被嵌合部品に嵌合する嵌合装置に関するもので、特に嵌合途中でつまりを生じた際にこれを解消する機能を備えた嵌合装置に関する。

機械部品の組立作業において頻繁に必要となる作業に、例えば、ワークを穴に嵌入する作業や、ギア部品の位相を合わせながら嵌め合いを行なう作業のように、ある部品（嵌合部品）を別の部品（被嵌合部品）に嵌合する作業がある。このような嵌合作業をロボットを利用して自動化することも知られている。一般に、嵌合作業の自動化は、嵌合部品と被嵌合部品の間の隙間が十分あれば容易であるが、隙間が狭い場合には工夫を要する。

嵌合部品と被嵌合部品の間の隙間が狭い場合の作業自動化のために従来より用いられていた技術としては、例えばロボットの先端にコンプライアンス機構を取り付け、位置・姿勢の誤差を機械的に吸収しながら挿入する方法や、力制御ロボットを用いて挿入する方法などがある。力制御ロボットでは、挿入中の力・モーメントを何らかの手段で検出し、それを減少させる方向へロボットを動作させることで位置・姿勢の誤差を修正し、挿入を円滑に行えるようにする（下記特許文献１を参照）。

特開平１１−１２３６８３号公報

しかし、上記のような技術を適用しても、嵌合部品と被嵌合部品の間の隙間が狭い場合には、嵌合の途中でつまりを全く発生しないようにすることは実際上困難である。そして、一旦つまりが発生すると、くさび効果により嵌合部品が挿入穴に対して強固に係合し、固定されてしまったような状態となる。上記特許文献１では、つまり発生時に引き抜き動作を試みることが提案されている。

しかし、つまりが頑固な場合も多く、そのような場合には、それ以上挿入を進めることも出来なければ、引き抜きも不可能となる。また、嵌合力あるいは引き抜きを大きくすると、部品を損傷するおそれもある。従来は、このような事態に対してアラーム停止で対処したため、システムの稼働率の低下の要因となっていた。そこで、本発明の目的は、嵌合途中でつまりを生じた際にこれを解消する機能を備えた嵌合装置を提供し、従来技術の問題点を解決することにある。

本発明は、ロボットが嵌合部品を把持して被嵌合部品に嵌合する嵌合装置に、嵌合途中でつまりが発生したことを検出するつまり検出手段と、つまりが検出されたとき嵌合を中断する手段と、つまりが発生した前記嵌合部品に対して、前記ロボットを用いて打撃力、あるいは、嵌合方向と交差する方向の外力の少なくとも一方を印加して前記つまりを解消する手段とを設けることで、上記課題を解決する。

つまり解消後には、嵌合を再開しても良く、嵌合部品を前記被嵌合部品から引き抜くようにしても良い。嵌合は、前記ロボットの手首部に設けられ力センサを用いた力制御により行われるものであってよい。その場合、打撃力の大きさを前記力センサで検出して制御することが出来る。また、嵌合は、前記ロボットの各軸に取り付けられたトルクセンサを用いて行われてもよい。その場合、前記打撃力の大きさを前記ロボットの各軸に取り付けられた前記トルクセンサで検出して制御することも出来る。

更に、嵌合は、前記手首部にかかる力を前記ロボットの各軸を駆動するモータの電流値から推定して行ってもよい。その場合、前記打撃力の大きさを前記ロボットの各軸を駆動するモータの電流値から推定して制御することも出来る。

また、つまりが発生したときに、前記嵌合部品に発生する力の方向またはモーメントの方向を検出し、該検出された力の方向またはモーメントの方向に基き、前記打撃力印加の方向を決定することも出来る。例えば、つまりが発生した前後における前記嵌合部品の位置と姿勢の変化した方向に基いて、前記打撃力の印加方向を決定することが出来る。
なお、つまり検出手段によるつまり発生の検出は、例えば、嵌合中の力やモーメントが所定値以上になっている状態で、嵌合が進行せず且つ嵌合部品の姿勢変化がないことを検出することを通して行なうことが出来る。あるいは、嵌合開始後、所定時間経過しても嵌合が完了しないことを検出して、つまり発生と判断するようにしてもよい。

このように、本発明の嵌合装置では、つまりが発生時には一旦挿入を中断し、ロボットを用いて嵌合部品に対して打撃（打撃力）を加えるか、あるいは、嵌合方向と交差するような方向の力を印加する。即ち、一旦つまりが発生して嵌合部品と被嵌合部品がかみあってしまった場合には、挿入方向あるいは引き抜き方向（挿入方向と平行で向きは逆）に力を加えても、くさび効果を解消させてつまりを解除することが困難であるが、それに交差するような方向の力を加えることで、つまり状態からの脱却を図ることが可能になる。

ロボットが力センサを備えている場合は、つまりが生じる直前に検出した力ベクトルから、つまりを解消するのに適当な打撃の方向を求める。また、各軸に取り付けられたトルクセンサがある場合は、トルクデータから嵌合部品にかかる力を計算し、打撃の方向を求めることができる。更に、モータ電流から各軸トルク、嵌合部品にかかる力を推定し、打撃の方向を求める方法もある。

部品によっては大きな打撃を加えると損傷する恐れがあるので、打撃力を制限することが望ましい。そのためには、例えば打撃力の大きさを力センサやロボットの各軸に取り付けたトルクセンサで測定したり、各軸のモータの電流値から推定したりしながらその値を制御すれば良い。

本発明により、部品の嵌合作業中につまりが発生しても、これを解消できるようになった。また、つまりが発生する度にアラーム停止することにより生じる稼働率の低下を回避することが可能になった。

図１は、本発明の一つの実施形態における配置の概要を示す図である。符号１は力制御ロボット本体で、ケーブル１１を介してロボットコントローラ２に接続されている。力制御ロボット１（以下、ロボット１と略記）の構造自体は従来のものと特に変わるところはない。また、ロボットコントローラ２についても、後述する処理を実行する為のソフトウェア手段を除けば、従来より使用されているものと同じで良い。

ロボット１の手首先端には、直交する３軸方向の並進力とそれらの軸回りのモーメント（まとめて６軸力と言う。）を検出できる６軸力センサ３が取り付けられている。符号３１は、力センサ３とロボットコントローラ２を接続するケーブルを表わしている。

作業対象の嵌合部品と被嵌合部品は、それぞれ符号４、５で示されている。本実施形態においては、ロボット１の手首先端に６軸力センサ３を介して取り付けられたハンド７で把持された嵌合部品４を作業テーブル６上に載置された被嵌合部品５に嵌合させる作業を行なう。但し、後述するように、嵌合途中でつまりが発生した場合には、嵌合部品４を被嵌合部品５から引き抜く作業を行うケースもあり得る。

ツール先端点は、嵌合部品４の先端面の中心に設定されているものとする。そして、挿入軸の方向（嵌合部品４を被嵌合部品５の開口部に挿入する際のロボット１の移動方向）が＋Ｙ軸方向に一致するように、ワーク座標系（ユーザ座標系）Σw が設定されているものとする。また、作業テーブル６上に供給される被嵌合部品５は、上記挿入軸の方向（Ｙ軸方向）に整合した姿勢で位置決めされているものとする。

図２は、図１中に示したロボットコントローラ２並びに６軸力センサ３を含む制御システムの概略構成を要部ブロック図で示したものである。同図を参照すると、システム全体は、ロボットコントローラ２、該ロボットコントローラ２によって力制御されるロボット（本体機構部）１、ロボット本体のアーム先端部の手首部に支持される６軸力センサ３、６軸力センサ３の検出信号を処理する信号処理装置３０から構成される。

ロボットコントローラ２は中央演算処理装置（以下、ＣＰＵという。）２１を備えている。ＣＰＵ２１には、ＲＯＭからなるメモリ２２、ＲＡＭからなるメモリ２３、不揮発性メモリ２４、ロボットの教示及び他のシステム各部の動作についての各種の指令入力あるいは設定値入力を行なうキーボードを備えた教示操作盤２５、信号処理装置３０との間のインターフェイス機能を果たす入出力装置２６、及びロボット本体１の各軸のサーボモータを駆動制御するサーボ回路２８を介して制御するロボット軸制御部２７が、各々バス２９を介して接続されている。

６軸力センサ３には、例えば歪ゲージで構成され発振器により交流駆動される複数のブリッジ回路を内蔵した公知の構造のものが使用出来る。６軸力センサ３からは、６軸力の各成分を表わす検出信号が信号処理装置３０に出力される。

信号処理装置３０に於ける信号処理の概略を例記すれば、次のようになる。６軸力センサ３から出力された各検出信号は、差動アンプで増幅後、同期整流されて直流信号に変換された上で信号処理装置内のマルチプレクサに入力される。マルチプレクサは、ロボットコントローラ２のＣＰＵ２１から入出力装置２６を介して受け取る制御信号に従い、各成分を表わす検出信号を順次サンプルホールド回路及びＡ／Ｄコンバータを介して入出力装置２６へ送り出す。

ＣＰＵ２１は、これを順次不揮発性メモリ２４内の所定領域に格納する。格納された６軸力センサ３の検出データは、ロボットの力制御の為に利用される。なお、力制御にはダンピング制御、インピーダンス制御などいくつかの手法がある。また、力センサの代わりに、ロボット１の各軸にトルクセンサを設け、トルクセンサの出力を用いて力制御を行なう方法や、各軸の電流値から各軸のトルクを推定して、それらの推定値を用いて力制御を行なう方法もある。

トルクセンサを利用する場合には、符号３で示した要素は各軸に設置されたトルクセンサとなり、符号３０で示した要素はそれらトルクセンサの検出信号を処理する信号処理装置となる。また、電流値からのトルク推定による方法を採用する場合には、これら要素は省くことができる。なお、ダンピング制御、トルクセンサを利用する方法、及び、電流値からのトルク推定による方法については、後述する。

ＲＯＭ２２、ＲＡＭ２３及び不揮発性メモリ２４は、上記６軸力センサ３の検出データの格納の他に、ロボットコントローラ２自身の動作を制御するシステムプログラム、後述するダンピング制御（あるいはそれに代わる力制御）の為の処理、信号処理装置３０との間の信号授受のコントロール等を実行するプログラム並びに各処理の関連設定値の格納等に用いられる。

この設定値には、６軸力センサ３に設定されたセンサ座標系とロボットに設定された直交座標系との間の変換行列のデータの他に、力制御（例えば、ダンピング制御）の諸パラメータが含まれる。

次に、上述した力制御ロボット１を用いて、被嵌合部品５の穴に嵌合部品４を嵌合する作業、及び、その際に発生し得るつまりの発生とその検出、つまり解消動作（外力印加）、つまり解消動作後の嵌合再開（または引き抜き）等について、図３以下を参照図に加えて説明する。なお、ここでは、嵌合部品４は円柱状の部品であり、これを円筒状の穴に挿入する場合について説明する。但し一般には、嵌合部品４の形状は円柱に限定されるわけではなく、四角桂や円錐など様々な形の部品のケースでも、本発明は同様に適用可能である。穴の形状についても、嵌合部品４の嵌入を許容する限り、特に制限はない。

図３（ａ）〜（ｄ）は、嵌合動作の直前から、嵌合完了までの推移の概略を示したものである。前述したように、ツール先端点は嵌合部品４の先端面の中心に設定されており、その位置を順にＰ0 、Ｐ1 、Ｐ2 、Ｐ3 で示した。なお、嵌合部品４を把持しているハンド７の図示は省略した。

先ず図３（ａ）は、嵌合動作の直前位置（姿勢も含む）として予めロボット１に教示されているアプローチ開始位置Ｐ0 にロボット１が位置決めされた状態を示す。アプローチ開始位置Ｐ0 は、嵌合部品４の先端面が、被嵌合部品５の穴の真上に位置し且つ同穴に正対姿勢をとるように教示されている。但し、実際には、個々の部品アイテム毎に被嵌合部品５の位置決め誤差があり、Ｐ0 の教示誤差等もあるため、嵌合部品４と被嵌合部品５の相対的な位置・姿勢の関係には、多少のばらつきがある。

次に、パラメータ設定されているアプローチ方向（嵌合方向で、図１における＋Ｙ軸方向）、押し付け目標力、アプローチ速度で速度及び力制御を行いながらアプローチ動作を開始する。やがて、図３（ｂ）に示したように被嵌合部品５に嵌合部品４が接触し、その反力が、パラメータ設定されているしきい値をこえると、被嵌合部品５と嵌合部品４が接触したものとして、図３（ｃ）に示す嵌合動作に移行する。この場合はパラメータ設定されている嵌合挿入方向、押し付け目標力、速度に基づいて、嵌合挿入方向（＋Ｙ軸方向）に速度、力制御をしながら嵌合部品４を移動させる。

嵌合が円滑に進み、アプローチ開始位置から測った＋Ｙ軸方向移動距離Ｌが予め設定した距離Ｌ0 に到達したら嵌合動作を終了する。図３（ｄ）はその状態を表わしている。図示は省略したが、その後、ハンド７を開き、嵌合部品４を解放し、ロボット１は退避する。なお、図３（ｄ）において、嵌合部品４の先端面と被嵌合部品５の穴の底面との間に隙間があるが、これは単なる例示である。

さて、図３（ａ）〜（ｄ）に示したように、嵌合が円滑に進めば問題はないが、既述の通り、実際にはつまりが発生し得る。そこで、本実施形態では、つまりが発生し得ることを前提に、図４（第１例）または図５（第２例）に示した処理フローで対処する。

第１例、第２例いずれの処理を行なう場合にも、まず、ロボット動作のための各種パラメータ値を予め設定し、不揮発性メモリ２４に記憶させておく。嵌合及び嵌合途中でのつまりによる引き抜き動作のために設定するパラメータには、アプローチ動作を規定するための挿入方向（図１に示す例ではワーク座標系のＹ軸方向）、アプローチ速度、挿入距離（図３に示したＬ0 ）の他、力制御を行うための諸パラメータが含まれる。

力制御の方式としてはインピーダンス制御やダンピング制御、またはハイブリッド制御などを用いることができ、いずれも公知であるが、ここでは一例として、ダンピング制御を採用することとする。その場合、力制御を行うための諸パラメータには、次のものがある。

Ｄd ：設定ダンパ値
Ｋd ：設定バネ定数値
Ｘd ：手先位置・姿勢（ツール先端点の位置・姿勢）の目標値
ｆd ：手先（ツール先端点）で発生する力の目標値
また、嵌合部品４と被嵌合部品５の接触を検出するために、力センサ３で検出される力のしきい値が設定される。更に、引き抜き動作時の引き抜き方向、目標速度、目標力、引き抜き完了位置等をも設定される。なお、嵌合途中での引き抜き動作においては、引き抜き方向は挿入方向の逆方向であり、目標速度、目標力も嵌合動作時の逆方向とすればよく、引き抜き完了位置もアプローチ開始位置とすればよいため、必ずしも引き抜き動作専用のパラメータとして設定しなくともよい。

後述する例では、引き抜き動作時の引き抜き方向、目標速度、目標力を嵌合動作時の挿入方向、目標速度、目標力を反転させたものとし、引き抜き完了位置はアプローチ開始位置としている。更に、つまり検出のためのパラメータとして、時間Ｔ1 、位置・姿勢の変化しきい値、力／モーメントのしきい値、あるいは、時間Ｔ0 を設定しておく。ここで、Ｔ1 と力／モーメントの基準値は図４（第１例）で用いるパラメータで、時間Ｔ1 の間、位置、姿勢が変化せず、且つ、力／モーメントが基準値を越えていれば、つまり発生と判断する。基準の例は後述する。時間Ｔ0 は、嵌合動作の完了に要する制限時間に対応する時間で、嵌合動作開始直前のタイマスタート時から時間Ｔ0 以内に嵌合が完了しなければ、つまり発生と判断する。なお、本実施形態では、例１、例２いずれにおいても、「つまり解消処理」後に、嵌合動作を再開して続行する、嵌合部品４の引き抜きを行なうかをレジスタ値でモード設定できるようにする。例えば、レジスタ値０なら嵌合動作再開とし、レジスタ値１なら引き抜き実行とする。

以上のとおり、各種パラメータ値を予め設定しておき、ロボット動作プログラムを実行する。ロボットコントローラ２のＣＰＵ２１は動作プログラムを順次１ブロックずつ読みだし、読み出したブロックの指令に基づいてロボット本体１の各軸サーボモータを駆動し、ロボット本体１を駆動する。そして、嵌合動作を開始するアプローチ開始位置Ｐ0 （図３参照；以下、同様）に位置決めされ、次のブロックを読みだし、「力嵌合指令」が読みだされると、ＣＰＵ２は、図４（第１例）または図５（第２例）に示す処理を開始する。先ず、第１例における各ステップの要点は下記の通りである。

ステップａ１；アプローチ開始位置（Ｐ0 ）より設定された挿入方向（図１の例ではＹ軸方向）に設定されたアプローチ速度で、力制御を行いながらハンドで把持された嵌合部品４を移動させる。この際、図１に示す例では、挿入方向のＹ軸並進方向のみ設定目標力による力制御がなされ、他の軸、及び回転軸に対しては力制御は無効にされ（力センサ３の出力を使わない）、移動指令もＹ軸並進方向のみ出力され、他の軸及び回転軸に対しては移動指令は出されず、一定の姿勢を保持する制御がなされる。被嵌合部品５に嵌合部品４が接触し、力センサ３で検出される力が設定されているしきい値以上に達すると、このアプローチ動作を終了する。

ステップａ２；タイマをＴ＝０に設定（クリア）し、スタートさせる。
ステップａ３；タイマスタート直後に、嵌合動作を開始（２回目の処理周期では「続行」あるいは「再開」）する。すなわち、設定されている挿入方向（Ｙ軸方向）に、設定速度及び設定目標力による速度制御、と力制御を行いながら移動させる。

前述したように、ここではダンピング制御を採用している。ダンピング制御は、公知の技術なので詳細の説明は省き、簡単に述べておく。ダンピング制御では、ロボットの手先（ツール先端点）が下記の式（１）、（２）を満たすように制御が行なわれる。なお、式（１）、（２）はＸ軸に関してのみ記載したが、実際はＸ、Ｙ、Ｚ、Ｘ回り、Ｙ回り、Ｚ回りの６方向について行う。式中の記号の内、Ｄd 、Ｋd 、Ｘd 、ｆd は、それぞれ前述したパラメータである。また、ｆは、それぞれ手先（ツール先端点）で発生している力で、力センサの検出結果から求められる。ｘは、実際に手先位置・姿勢（ツール先端点の位置・姿勢）である。

上記式（１）、（２）に従った制御により、前出の図３（ｂ）のように、初期位置誤差・姿勢誤差がある状態で挿入しても、その誤差を減らすように嵌合部品４を動かし、後述するつまりが発生しない限り、嵌合部品４が被嵌合部品５の穴１０内に徐々に挿入されていく。

ステップａ４／ａ５；つまりが発生していないかチェックするために、時間Ｔ1 が経過したら、その間のロボットの位置、姿勢の変化の有無をチェックする。例えばツール先端点の位置、姿勢の６成分の内、１成分も変化がしきい値以下であれば、「位置、姿勢変化なし」と判定してステップａ６へ進む。それ以外は、「位置、姿勢変化あり」と判定してステップａ１０へ進む。

ステップａ６；力／モーメントが基準を越えているかチェックする。例えば、ツール先端点に作用する６軸力の一成分でも、しきい値を越えていれば、つまり発生と判断して、ステップａ７へ進む。それ以外はステップａ１０へ進む。

ステップａ７；嵌合動作を停止し、つまり解消の処理を行なう。つまり解消の処理は、本発明に特有のもので、内容は後述する。
ステップａ８；引き抜きか、嵌合動作再開かを指定したレジスタ値をチェックし、引き抜き指定ならステップａ９へ進み、そうでなければステップａ９へ進む。

ステップａ９；引き抜き処理を行なう。具体的には、現在位置とアプローチ開始位置Ｐ0 との距離を算出し、挿入方向と逆方向に同距離だけ力制御装置で移動する。即ち、移動方向、目標速度、目標力を、設定されている嵌合動作時の挿入方向、目標速度、目標力を反転させたものとし、嵌合動作と同様の力及び速度制御を行いながら引き抜き動作を実行する。移動を干渉したら、処理を終了する。

ステップａ１０；嵌合動作移動量Ｌ0 だけの移動が完了したか否かチェックし、Ｌ0 以上移動していれば嵌合完了と判断して処理を終了する。もし、移動未完了であれば、ステップａ２へ戻り、タイマをＴ＝０に設定（クリア）し、スタートさせ、次いでステップａ３で嵌合動作を続行（または再開）する。以下、ステップａ４以下を繰り返す。いずれにしろ、嵌合途中で１回またはそれ以上、つまりが発生しても、ステップａ７でつまり解消の処理が行なわれるので、ステップａ１０でイエスが出力されるか、ステップａ９で引き抜きが行なわれるかして、処理が終了される。

第２例（図５）のフローを採用する場合の各ステップの要点は下記の通りである。
ステップｂ１；アプローチ開始位置（Ｐ0 ）より設定された挿入方向（図１の例ではＹ軸方向）に設定されたアプローチ速度で、力制御を行いながらハンドで把持された嵌合部品４を移動させる。この際、図１に示す例では、挿入方向のＹ軸並進方向のみ設定目標力による力制御がなされ、他の軸、及び回転軸に対しては力制御は無効にされ（力センサ３の出力を使わない）、移動指令もＹ軸並進方向のみ出力され、他の軸及び回転軸に対しては移動指令は出されず、一定の姿勢を保持する制御がなされる。被嵌合部品５に嵌合部品４が接触し、力センサ３で検出される力が設定されているしきい値以上に達すると、このアプローチ動作を終了する。

ステップｂ２；タイマをＴ＝０に設定（クリア）し、スタートさせる。
ステップｂ３；タイマスタート直後に、嵌合動作を開始（２回目の処理周期では「続行」あるいは「再開」）する。すなわち、設定されている挿入方向（Ｙ軸方向）に、設定速度及び設定目標力による速度制御、と力制御（ダンピング制御）を行いながら移動させる。ダンピング制御については、第１例のステップａ３で説明した通りである。

ステップｂ４；嵌合動作移動量Ｌ0 だけの移動が完了したか否かチェックし、Ｌ0 以上移動していれば嵌合完了と判断して処理を終了する。移動未完了であれば、ステップｂ５へ進む。
ステップｂ５；嵌合動作の制限時間Ｔ0 が満了していないかどうかチェックし、満了していれば、つまり発生と判断してステップｂへ進む。未満了であれば、テップｂ３へ戻り、嵌合動作を続行する。

ステップｂ６；嵌合動作を停止し、つまり解消の処理を行なう。つまり解消の処理の内容については、後述する。
ステップｂ７；引き抜きか、嵌合動作再開かを指定したレジスタ値をチェックし、引き抜き指定ならステップｂ８へ進み、そうでなければステップｂ３へ戻り、嵌合動作を再開する。

ステップｂ８；引き抜き処理を行ない、処理を終了する。処理内容は第１例のステップａ９で説明した通りである。
本例のフローにおいても、嵌合途中で１回またはそれ以上、つまりが発生した場合に、ステップｂ６でつまり解消の処理が行なわれるので、ステップｂ４でイエスが出力されるか、ステップｂ８で引き抜きが行なわれるかして、処理が終了される。

なお、上述した実施形態では、手先（ツール先端点）で発生している力を知るために力センサ３を用いているが、代替手法によってもよい。代替手法の１つは、各軸にトルクセンサを設け、各軸トルクの大きさからツール先端点での力を計算する手法である。周知のように、各軸トルクとツール先端点での力・モーメントとの間には、下記（３）式のような関係がある。

τ＝Ｊ^T Ｆ・・・・・（３）
但し、
τ：各軸のトルクτ1 〜τ6 を成分とするベクトル
Ｊ：各軸値θ1 〜θ6 とツール先端点の位置・姿勢（Ｘ、Ｙ、Ｚ、Ｐ、Ｗ、Ｒ）の間の微分変換関係を表わすヤコビ行列で、その計算に必要なパラメータデータは予めロボットコントローラ２のメモリに格納されている。符号Ｔは転置行列を表わす。
Ｆ：ツール先端点における６軸力（ＦX 、ＦY 、ＦZ、ＭX 、ＭY 、ＭZ ）を成分とするベクトル
この（３）式の関係を使えば、ツール先端点における６軸力Ｆは下記（４）式で求められる。
Ｆ＝（Ｊ^T ）^-1τ ・・・・・（４）
従って、力センサを用いたと同様の方式で、力制御を行い、嵌合動作を実行できる。但し、ヤコビ行列の計算が必要なことから、ヤコビ行列の計算ができないような特異な姿勢で嵌合動作を行なわないことが条件になる。

もう１つの代替手法は、各軸のモータの電流直から発生トルクを推定するものである。モータに流れる電流値と発生トルクは比例するので、電流値を検出することで、下記（５）式により、発生トルクがわかる。
τi ＝Ｋ・Ｉi ・・・・・（５）
但し、
τi ：第ｉ軸のトルク
Ｉi ：第ｉ軸のモータの電流値
この（５）式の関係を使っても、力センサを用いたと同様の方式で、力制御を行い、嵌合動作を実行できる。なお、各軸がダイレクトドライブでなく減速機のあるロボットの場合は、摩擦で失われるトルクが大きいので、摩擦力をあらかじめ測定したものをテーブルで持ったり、摩擦力を推定する手法などと組み合わせることが望ましい。摩擦を考慮したトルクは、下記（６）式で表わされる。
τir＝τi −τif ・・・・・（６）
但し、
τi ：第ｉ軸のモータで生成されるトルク
τir：第ｉ軸の駆動に実際に使用されるトルク
τif：第ｉ軸の摩擦で失われるトルク
なお、摩擦力を求める手法は種々あり、公知なのでここではその説明は省略する。

次に、上記説明した第１例におけるステップａ７、第２例におけるステップｂ６で行なわれるつまり解消処理について説明する。
［つまり解消処理例１］
つまり解消のための最も簡便な方法は、嵌合部品４の挿入方向と交差する任意のの方向に打撃力を加えることである。例えば、図６に示したように、Ｙ軸（挿入方向）と直交する方向に打撃力（矢印Ｓ１、Ｓ２）を加えれば、かなりのケースにおいて、衝撃により、嵌合部品４と被嵌合部品５の頑固なつまり状態は解除できる。打撃力は、図示した矢印Ｓ１、Ｓ２のように、逆方向から２回加えることで、いずれかの打撃で接触点１、２を離隔させ、つまり状態解除の確度を上げることが可能となる。このような打撃力を生み出すには、例えばロボットコントローラ２内で、高ゲイン条件下で、小距離＋ΔＸの移動指令と−ΔＸの移動指令を短時間間隔で出力すればよい。あるいは、目標力ｆd を適切な打撃力に設定して、ダンピング制御によりロボットを一瞬だけ動作させてもよい。この場合、打撃力がｆd に制限されるので、過剰な打撃力により部品、ハンド、力センサ等を損傷することが防止される。なお、ここで力制御は、前述した通り、ツール先端点における力を、力センサ出力から計算する手法の他、各軸のトルクセンサ出力から計算する手法や、各軸モータの電流値から推定するものでもあってよい。

［つまり解消処理例２］
打撃を加える方向を特定し、なるべく効果的な方向からの打撃を実行することも可能である。図７は、２点接触でつまりが生じているときに、接触点１、２を通り、押し付け方向ベクトルに平行な平面で切り取った断面を示している。以下この平面上で力の働き方を考える。発生している力とその作用方向は、矢印で示されている。図中φ、θは、それぞれ同図が示す面内で、円柱形状の嵌合部品の傾き角、及び、接触点１、２を通る直線が円柱形状の嵌合部品の中心軸方向となす角度を表わしている。また、μは、接触点１、２で嵌合部品と被嵌合部品の間の摩擦の摩擦係数を表わす。なお、ここでは鉛直下方向に嵌合部品４を挿入する場合を例にとっているが、挿入方向が鉛直下向きでない場合も、重力の方向を変えれば同様に考えることができる。

図７に示したつまり状態で発生している力について、釣り合いの式を考える。先ず、押し付け方向（挿入方向）の釣り合いの式ば、
Ｆz ＋Ｍg ＋Ｎ1 cos θ −Ｎ2 cos θ＝Ｒ1 ＋Ｒ2
となる。ここで、
●Ｎ1 ：接触点１で発生する力
●Ｎ2 ：接触点２で発生する力
●Ｒ1 ＝μ・Ｎ1 ・sin θ：接触点１で発生する摩擦力
●Ｒ2 ＝μ・Ｎ2 ・sin θ：接触点２で発生する摩擦力
●Ｍg ：重力
である。なお、ここでは、嵌合部品の傾き角φがあまり大きくなく、摩擦力Ｒ1 とＲ2 は鉛直方向に発生すると仮定している。

横方向の釣り合いの式は、
Ｆ＝Ｎ1 sin θ−Ｎ2 sin θ
ここで、Ｆは押し付け力Ｆz をかけることによって生じる横方向の力である。
さて、つまりが発生している状況では、接触点１から接触点２へ向かう力Ｎ1 と接触点２から接触点１へ向かう力Ｎ2 は一般に等しくなく、両差の差がくさび力となる。

図８は、図７に示したつまり発生状態を上方から見た図である。横方向の力はくさび力だけなので、接触点１と接触点２を結ぶ線と平行に力Ｆが発生する。上述した実施形態のように、力センサがある場合には、力センサによる検出結果から、この力Ｆの大きさと方向を求めることが出来る。即ち、接触点１から接触点２へ向かう直線を、挿入方向と垂直な平面へ射影した方向がわかる。そこで、ロボットを用いて、この力Ｆと平行な方向（接触点１から接触点２へ向かう直線を、挿入方向と垂直な平面へ射影した方向）に力を加えることで、つまりを最も効果的に解消させることができる。

そのような打撃力を生み出すには、例えばロボットコントローラ２内で、高ゲイン条件下で、上記方向（接触点１から接触点２へ向かう直線を、挿入方向と垂直な平面へ射影した方向）への小距離δの移動指令を短時間間隔で出力すればよい。あるいは、適切な大きさで上記方向を向いた目標力ｆd が合力として得られるように、Ｘ軸方向の目標力とＹ軸方向の目標力を設定し、ダンピング制御によりロボットを一瞬だけ動作させてもよい。この場合も、打撃力がｆd に制限されるので、過剰な打撃力により部品、ハンド、力センサ等を損傷することが防止される。なお、ここで力制御は、前述した通り、ツール先端点における力を、力センサ出力から計算する手法の他、各軸のトルクセンサ出力から計算する手法や、各軸モータの電流値から推定するものでもあってよい。

［つまり解消処理例３］
力・モーメントから接触点を結ぶ方向の情報を得る代わりに、位置の変化量からそれを求める方法もある。押し付け方向がＺ方向である場合、初期位置・姿勢（アプローチ開始位置尾Ｐ0 ）からつまりが発生する直前までの嵌合部品４のＸ方向位置及びＹ方向位置の変化量を（△ｘ、△ｙ）とした時、−（△ｘ、△ｙ）方向が、「接触点１から接触点２へ向かう直線を、挿入方向と垂直な平面へ射影した方向」に対応していると考えられる。そこで、この−（△ｘ、△ｙ）の方向に、打撃力を加えることでつまりを効果的に解消することが出来る。なお、そのような打撃力を生み出す方法は、つまり解消処理例２と同様なので、繰り返し説明は省略する。

本発明の実施形態の配置の概要を示す図である。本発明の実施形態におけるロボットコントローラの構成ブロック図である。本発明における嵌合動作の説明図である。本実施形態における処理フローの例（第１例）を示すフローチャートである。本実施形態における処理フローの別の例（第２例）を示すフローチャートである。打撃を加えてつまりを解消する方法について説明する図である。打撃を加える方向を特定してつまりを解消する方法について説明する図で、２点接触でつまりが生じているときに、接触点１、２を通って押し付け方向ベクトルに平行な平面で切りとった断面が示されている。図７に示したつまり発生状態を上方から見た図である。

符号の説明

１ロボット（力制御ロボットの本体機構部）
２ロボットコントローラ
３力センサ
４嵌合部品
５被嵌合部品
７ハンド
２１ＣＰＵ
２２ＲＯＭ
２３ＲＡＭ
２４不揮発性メモリ
２５教示操作盤
２６入力出力装置
２７ロボット軸制御部
２８サーボ回路
３０信号処理装置

Claims

ロボットが嵌合部品を把持して被嵌合部品に嵌合する嵌合装置において、
嵌合途中でつまりが発生したことを検出するつまり検出手段と、
つまりが検出されたとき嵌合を中断する手段と、
つまりが発生した前記嵌合部品に対して、前記ロボットを用いて打撃力、あるいは、嵌合方向と交差する方向の外力の少なくとも一方を印加して前記つまりを解消する手段と、
前記嵌合を再開する手段とを備えた嵌合装置。
ロボットが嵌合部品を把持して被嵌合部品に嵌合する嵌合装置において、
嵌合途中でつまりが発生したことを検出するつまり検出手段と、
つまりが検出されたとき嵌合を中断する手段と、
つまりが発生した前記嵌合部品に対して、前記ロボットを用いて打撃力、あるいは、嵌合方向と交差する方向の外力の少なくとも一方を印加して前記つまりを解消する手段と、
嵌合途中の前記嵌合部品を前記被嵌合部品から引き抜く手段とを備えた嵌合装置。
前記嵌合は、前記ロボットの手首部に設けられ力センサを用いた力制御により行われることを特徴とする、請求項１又は請求項２に記載の嵌合装置。
前記打撃力の大きさを前記力センサで検出して制御することを特徴とする、請求項３に記載の嵌合装置。
前記嵌合は、前記ロボットの各軸に取り付けられたトルクセンサを用いて行うことを特徴とする、請求項１又は請求項２に記載の嵌合装置。
前記打撃力の大きさを前記ロボットの各軸に取り付けられた前記トルクセンサで検出して制御することを特徴とする、請求項５に記載の嵌合装置。
前記嵌合は、前記手首部にかかる力を前記ロボットの各軸を駆動するモータの電流値から推定して行うことを特徴とする、請求項１又は請求項２に記載の嵌合装置。
前記打撃力の大きさを前記ロボットの各軸を駆動するモータの電流値から推定して制御することを特徴とする、請求項７に記載の嵌合装置。
つまりが発生したときに、前記嵌合部品に発生する力の方向またはモーメントの方向を検出し、該検出された力の方向またはモーメントの方向に基き、前記打撃力印加の方向を決定することを特徴とする、請求項１乃至請求項８の内、何れか１項に記載の嵌合装置。
つまりが発生した前後における前記嵌合部品の位置と姿勢の変化した方向に基いて、前記打撃力の印加方向を決定することを特徴とする、請求項１乃至請求項８の内、何れか１項に記載の嵌合装置。
前記つまり検出手段は、嵌合中の力やモーメントが所定値以上になっている状態で、嵌合が進行せず且つ嵌合部品の姿勢変化がないことを検出することを特徴とする、請求項１乃至請求項１０の内、何れか１項に記載の嵌合装置。
前記つまり検出手段は、嵌合開始後、所定時間経過しても嵌合が完了しないことを検出することを特徴とする、請求項１乃至請求項１０の内、何れか１項に記載の嵌合装置。