(57)【要約】      （修正有）【課題】挿入動作の無駄な探索を無くし、探索時間を短縮できるようにする。【解決手段】ワークを対象物に押し当てながらワークの位置を移動させる処理と、ワークを対象物へ挿入する挿入位置を探索する処理と、ワークを対象物へ挿入する処理と、ワークを対象物へ挿入完了したことを判断する処理とを備えた組み立て作業ロボットの制御方法において、探索動作開始時にワークを対象物に押し付けて（Ｓ１０２）、ロボットに取り付けた力・モーメントセンサが検出するモーメントの値から挿入中心位置の探索方向を決定して（Ｓ１０３〜Ｓ１０５）、探索動作を開始するようにする。【選択図】図２
2009061550.tif 000002
【特許請求の範囲】
【請求項１】
ワークを対象物に押し当てながら前記ワークの位置を移動させるワーク移動ステップと、前記ワークを対象物へ挿入する挿入位置を探索する挿入位置探索ステップと、前記ワークを前記対象物へ挿入するワーク挿入ステップと、前記ワークを前記対象物へ挿入完了したことを判断する挿入完了判断ステップとを備えた組み立て作業ロボットの制御方法において、
前記挿入位置探索ステップは、探索動作開始時に前記ワークを前記対象物に押し付けて、ロボットに取り付けた力・モーメントセンサが検出するモーメントの値から挿入中心位置の探索方向を決定して、探索動作を開始することを特徴とする組み立て作業ロボットの制御方法。
【請求項２】
前記挿入位置探索ステップは、探索動作途中に前記モーメントを検出しながら、探索方向を逐次変更することを特徴とする請求項１記載の組み立て作業ロボットの制御方法。
【請求項３】
前記挿入位置探索ステップは、前記ワークを前記対象物に押し付けながら前記ワークを平面内で振動させて、さらに振動方向を平面内で回転させながら探索することを特徴とする請求項１記載の組み立て作業ロボットの制御方法。
【請求項４】
前記挿入位置探索ステップは、前記ワークを傾けて振動させながら探索することを特徴とする請求項３記載の組み立て作業ロボットの制御方法。
【請求項５】
前記ワークを対象物に押し当てながら前記ワークの位置を移動させるワーク移動部と、  前記ワークを前記対象物へ挿入する挿入位置を探索する挿入位置探索部と、前記ワークを前記対象物へ挿入するワーク挿入部と、前記ワークを前記対象物へ挿入完了したことを判断する挿入完了判断部とを備えた組み立て作業ロボットの制御装置において、
前記挿入位置探索部は、探索動作開始時に前記ワークを前記対象物に押し付けて、ロボットに取り付けた力・モーメントセンサが検出するモーメントの値から挿入中心位置の探索方向を決定して、探索動作を開始することを特徴とする組み立て作業ロボットの制御装置。
【請求項６】
前記挿入位置探索部は、探索動作途中に前記モーメントを検出しながら、探索方向を逐次変更することを特徴とする請求項５記載の組み立て作業ロボットの制御装置。
【請求項７】
前記挿入位置探索部は、前記ワークを前記対象物に押し付けながら前記ワークを平面内で振動させて、さらに振動方向を平面内で回転させながら探索することを特徴とする請求項５記載の組み立て作業ロボットの制御装置。
【請求項８】
前記挿入位置探索部は、前記ワークを傾けて振動させながら探索することを特徴とする請求項７記載の組み立て作業ロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、機械部品の組み立てなどにおいて、ピン形状のワークを対象物の穴に挿入する作業や、コネクタ挿入のようにワークの凹凸部を対象物の凹凸部へ嵌め込む作業をロボットで行うための組み立て作業ロボットの制御方法、制御装置に関する。
【背景技術】
【０００２】
図１０は従来の組み立て作業ロボットの装置の配置図である。１００１はロボットの制御装置であり、１００２はロボットであり、１００３はワークをつかむグリッパであり、１００４は力・モーメントセンサであり、１００７は対象物が固定されているテーブルであり、１００８は対象物へ挿入する部品であるワークであり、１００９はワークを挿入する対象物であり、１０１０は対象物にあるワークを挿入する穴である。グリッパ１００３を含むロボット１００２は制御装置１００１とケーブル１００５で接続され、力・モーメントセンサ１００４は制御装置１００１とケーブル１００６で接続されている。
図１１は図１０で使用される制御装置１００１の内部のブロック図の例である。制御装置１００１はメインコントロール部１１０１、操作盤インタフェース部１１０７、Ｉ／Ｏ部１１０６、サーボ制御部１１０５から構成され、バスで接続されている。メインコントロール部１１０１の内部にはＣＰＵ１１０２、メモリ１１０３、バスブリッジ１１０４がある。操作盤インタフェース部１１０７は操作盤１１０８と接続され、Ｉ／Ｏ部１１０６はロボットに取り付けられた力・モーメントセンサ１００４とグリッパ１００３と接続され、サーボ制御部１１０５はロボットの各軸のモータ１１０９と接続される。
制御装置はロボットへ動作指令である位置、速度、トルクなどを与え、ロボットからフィードバック位置、速度、トルクなどを受け取る。また、制御装置はセンサからＸ、Ｙ、Ｚ方向の力とＸ、Ｙ、Ｚまわりのモーメントを受け取る。また、制御装置はグリッパに対して開閉の信号を出力する。制御装置はソフトウェアでインピーダンス制御などの力制御の処理を行うとともに、組み立て動作に関する処理を行って、ロボットに動作指令を出す。ロボットは制御装置からの指令により、ワークを対象物に挿入する組み立て動作を行うようになっている。
従来の組み立て作業ロボットの制御方法は、ロボットの手先に力・モーメントセンサを備え、ロボットの手先が外力に対して柔軟に動作するインピーダンス制御などの力制御を行うようにし、ワークを対象物に押し当てながら挿入する位置を探索して挿入して組み立てる作業において、探索方向を並進成分と座標軸まわりの回転成分で表したとき、各成分ごとにあらかじめ探索範囲を決めて、各成分の探索範囲を往復させ、挿入位置を探索している（例えば、特許文献１参照）。前記探索範囲は、穴の寸法情報をもとに挿入可能範囲を導き出し、前記挿入可能範囲と位置決め誤差を考慮して決める。ワークを対象物に押し付けながら探索範囲を往復させることで挿入可能範囲の探索を行い、探索の途中で探索方向の力またはモーメントを検出すると、その探索方向の移動を停止して、他の探索方向への移動は継続し、挿入可能範囲にワーク中心が到達すると、穴の縁が面取りされていることと押し付けていることによって挿入が可能となっている。
また、ワークを直進させ、進行方向から反力を受けた場合には、その進行方向への移動を停止し、ランダムに横方向にワークを動かすことで挿入位置を探索し、（１）穴を探す、（２）姿勢を変える、（３）直進させる、（４）挿入の完了を判定する、の４つの基本動作要素によってワークを穴へ挿入しているものもある（例えば、特許文献２参照）。
図１２は対象物１００９を上から見た図であり、特許文献１の方法を示している。１２０１は穴の中心であり、１２０５は挿入可能範囲の半径である。１２０６は、押し付け開始位置であるとともに挿入可能範囲の探索を開始する位置であり、１２０７の点線は探索の経路を示している。前記挿可能範囲１２０５は、穴の半径ｒ１２０２、面取り量ｃ１２０４、ワークと穴のクリアランス量をεとすると、穴の中心から半径ｒ＋ｃ＋ε/２の範囲として決めてある。１２０８はＸの探索範囲でありＸＬからＸＵまで、１２０９はＹの探索範囲でありＹＬからＹＵまでとあらかじめ決めてある。
まず、ロボットは手先に力・モーメントセンサを備え、制御装置でロボットの手先が外力に対して柔軟に動作するようにインピーダンス制御している状態で、あらかじめ教示された穴の上部に位置決めした後、ワークを下げて対象物へ押し付ける。
次に、ロボットはワークを探索方向へ移動させる。図１２では、探索方向はＸとＹのみの２次元平面内で行う場合であり、１２０６の位置における最初の探索方向は、Ｘには現在位置からＸＵへ向かう方向、Ｙには現在位置からＹＬへ向かう方向である。ＸはＸＵまで行き着いたらＸＬ方向へ反転し、ＹはＹＬまで行き着いたらＹＵ方向へ反転する。同様に、ＸはＸＬまで行き着いたらＸＵ方向へ反転し、ＹはＹＵまで行き着いたらＹＬ方向へ反転することで、往復動作をする。往復動作の周波数はＸとＹで異なるようにし、Ｘを最も速く移動するようにし、ＸがＸＬからＸＵまで移動する間に、Ｙは前記挿入可能範囲であるｒ＋ｃ＋ε/２だけ移動するようにすると、ワーク中心は図中の探索範囲内の点線の経路１２０７を移動することになる。ワーク中心１２０３が前記経路を移動することで挿入位置を探索する。
探索の途中で探索方向の力またはモーメントを検出すると、その探索方向の移動を停止して、他の探索方向への移動は継続する。挿入可能範囲にワーク中心が到達すると、押し付け力によってワークにはＸとＹに垂直な方向である挿入方向への力がかかっており、穴には面取りがしてあるために、対象物からの反力によってワークは穴に入る方向へ動くことになるので、ワークは穴に挿入され、前記制御装置は垂直方向の位置または力の変化によってワークが穴へ挿入されたことを検出し、作業を終了する。
図１３は特許文献２の方法を示している。１３０１はロボットが把持するワークであり、１３０２は対象物であり、１３０３は対象物の穴である。ロボットは、反力を受けない限りワーク１３０１を直進させて対象物へ押し当て、押し当てたことでワークに反力が働いたら対象物表面の平面上でランダムな横方向へ進め、穴へ接触してモーメントを検出するとモーメントを減少させる方向にワーク姿勢を変化させ、穴底部へ着いて径方向へ動かすとどの方向からも反力があるので挿入完了を判断して動作を終了する、という動作によって１３０４から１３１２までの経路をたどり、ワークが穴へ挿入されるようになっている。
このように、従来の組み立て作業ロボットの制御方法では、あらかじめ決めた探索範囲を往復させることでワークを挿入可能範囲へ到達させて押し付け力で挿入したり、力・モーメントセンサに反力が検出される場所を探索し、力・モーメントセンサが検出した値を条件として、ランダムな横方向移動や姿勢変化などのワークの動作をすることによって最終的な挿入位置を探索して挿入して組み立てる、という手順がとられていた。
【特許文献１】特開２００４−１６７６５１号公報
【特許文献２】特開平７−３１４２６２号公報（第５頁、[００３９]）
【発明の開示】
【発明が解決しようとする課題】
【０００３】
従来の組み立て作業ロボットの制御方法は、探索動作において、穴の面取り量が小さい場合は、押し付け力と対象物からの反力によってワークが穴中心へ向かうとは限らず、押し付けのみでは挿入できないことがある。探索の途中で探索方向の力またはモーメントを検出すると、その探索方向の移動を停止して他の探索方向への移動は継続した場合にも、探索方向が穴の中心に向かわないと探索動作が長くなり、挿入可能範囲へ到達するまでに時間がかかるという問題があった。
また、ワークと対象物の寸法情報からあらかじめ決める挿入可能範囲は、穴およびワークの形状が円柱ではなく四角柱などの場合には、ワークのＺまわりの回転角が対象物の穴の形状と合う角度になっていないと、押し付けのみでは挿入することが困難である。四角柱の場合は並進方向だけではなく回転方向の範囲を決めなければならず、さらに探索方向で速く移動する順番も決めなければならないが、対象に適した範囲を決めることが容易ではない。
どのように設定するかについては、特許文献１では、ギアのように同じパターンがワーク円周上に繰り返されている形状の説明にとどまっており、四角柱などについては明確に記載されていない。たとえば、図１４のように四角柱の場合は、Ｚまわり回転の挿入可能範囲１４０６はθであるが、探索開始時のワーク１４０４のＺまわり回転角１４０７が穴１４０２よりもθ以上ずれていた場合には、探索方向のＺまわり回転成分をθずつ変化させて探索しても、ＸとＹも同時に動くために挿入可能な位置へ到達するまでの時間が長くなり、挿入可能範囲からあらかじめ適当な探索範囲を決めることは困難である。
また、特許文献２の反力を検出したらランダムな横方向に動作させる方法では、探索方向が穴中心に向くまでに時間がかかる場合があり、さらに形状が円柱ではない場合には挿入可能な位置を見つけられないことがある。このように、四角柱などワークのＺまわりの回転角を対象物の形状と合わせなければならない場合には、押し付けのみでは挿入することが困難であるという問題があった。
また、ワーク側と対象物の挿入位置に凹凸がある、コネクタなどの挿入の場合には、平面内で往復動作させると、挿入可能な位置ではないのに引っかかりによって平面内で往復動作ができなくなるという問題もあった。
本発明はこのような問題点に鑑みてなされたものであり、センサが検出したモーメントによって穴の中心方向を推定して探索方向を限定することで、探索時間を短縮するとともに、平面内を振動させる探索をすることでワークを対象物の穴へ挿入することができる組み立て作業ロボットの制御方法および制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００４】
上記問題を解決するため、請求項１に記載の組み立て作業ロボットの制御方法の発明は、ワークを対象物に押し当てながら前記ワークの位置を移動させるワーク移動ステップと、前記ワークを対象物へ挿入する挿入位置を探索する挿入位置探索ステップと、前記ワークを前記対象物へ挿入するワーク挿入ステップと、前記ワークを前記対象物へ挿入完了したことを判断する挿入完了判断ステップとを備えた組み立て作業ロボットの制御方法において、前記挿入位置探索ステップが、探索動作開始時に前記ワークを前記対象物に押し付けて、ロボットに取り付けた力・モーメントセンサが検出するモーメントの値から挿入中心位置の探索方向を決定して、探索動作を開始することを特徴としている。
また、請求項２記載の発明は、請求項１記載の組み立て作業ロボットの制御方法において、前記挿入位置探索ステップが、探索動作途中に前記モーメントを検出しながら、探索方向を逐次変更することを特徴としている。
また、請求項３記載の発明は、請求項１記載の組み立て作業ロボットの制御方法において、前記挿入位置探索ステップが、前記ワークを前記対象物に押し付けながら前記ワークを平面内で振動させて、さらに振動方向を平面内で回転させながら探索することを特徴としている。
また、請求項４記載の発明は、請求項３記載の組み立て作業ロボットの制御方法において、前記挿入位置探索ステップが、前記ワークを傾けて振動させながら探索することを特徴としている。
また、請求項５記載の組み立て作業ロボットの制御装置の発明は、前記ワークを対象物に押し当てながら前記ワークの位置を移動させるワーク移動部と、前記ワークを前記対象物へ挿入する挿入位置を探索する挿入位置探索部と、前記ワークを前記対象物へ挿入するワーク挿入部と、前記ワークを前記対象物へ挿入完了したことを判断する挿入完了判断部とを備えた組み立て作業ロボットの制御装置において、前記挿入位置探索部が、探索動作開始時に前記ワークを前記対象物に押し付けて、ロボットに取り付けた力・モーメントセンサが検出するモーメントの値から挿入中心位置の探索方向を決定して、探索動作を開始することを特徴としている。
また、請求項６記載の発明は、請求項５記載の組み立て作業ロボットの制御装置において、前記挿入位置探索部が、探索動作途中に前記モーメントを検出しながら、探索方向を逐次変更することを特徴としている。
また、請求項７記載の発明は、請求項５記載の組み立て作業ロボットの制御装置において、前記挿入位置探索部は、前記ワークを前記対象物に押し付けながら前記ワークを平面内で振動させて、さらに振動方向を平面内で回転させながら探索することを特徴とする請求項５記載の組み立て作業ロボットの制御装置。
また、請求項８記載の発明は、請求項７記載の組み立て作業ロボットの制御装置において、前記挿入位置探索部が、前記ワークを傾けて振動させながら探索することを特徴としている。
【発明の効果】
【０００５】
本発明によると、穴中心方向を推定して探索方向を限定することができ、挿入可能範囲に到達しているのに穴中心から遠ざかるような、無駄な探索動作をなくすので探索時間を短縮することができる。
また、ワークを移動しながら穴中心方向へ探索方向を変更することができ、常に穴の中心へ向かうように動作することで、無駄な探索動作をなくすので探索時間を短縮することができる。
また、穴の形状が円柱だけではなく、押し付け力のみで挿入するのが困難な四角柱などでも挿入できる位置を探索して挿入することができる。
また、コネクタなど挿入位置に凹凸がある対象物でも、引っかかりを避けて、挿入できる位置を探索して挿入することができる。
【発明を実施するための最良の形態】
【０００６】
以下、本発明の方法の具体的実施例について、図に基づいて説明する。
【実施例１】
【０００７】
図１は本発明の実施例１に係る組み立て作業ロボットの制御装置の探索動作を示す図であり、対象物およびワークを上から見た図である。図２は実施例１に係る組み立て作業ロボットの制御のフローチャートである。本発明の方法を実施する装置の配置は、従来の装置の配置である図１０と同じである。
図１において、２０１は対象物であり、２０２は対象物上の穴であり、２０３は対象物の穴の中心であり、２０４は穴の半径に穴の面取り量を足した挿入可能範囲であり、２０５はワークであり、２０６はワーク中心である。２０７は穴の中心方向で、２０８はツール座標系で２０９ＭｘはＸ軸まわりのモーメントであり、２１０ＭｙはＹ軸まわりのモーメントである。本発明の制御方法を実施する制御装置のソフトウェアは、インピーダンス制御処理部と、組み立て動作処理部（例えば、Ｓ１０１〜Ｓ１０７の処理、特にＳ１０２〜Ｓ１０５の処理が制御装置の組み立て動作処理部に相当する）を備えている。
本発明が従来技術と異なる部分は、後述の制御装置の組み立て動作処理部に力・モーメントセンサから得られたモーメントを利用して探索方向の決定をする処理を備えた部分である。
【０００８】
本実施例では、探索を開始する最初の押し付け位置でのワーク中心が、穴の挿入可能範囲に入るように初期位置を教示しているものとし、最初の押し付け位置において、力・モーメントセンサから０でないモーメントが検出される場合について説明する。初期位置が穴の挿入可能範囲に無いところから始まる場合には、従来の探索方法で穴の縁にかかるまで探索すれば良い。
【０００９】
つぎに動作について説明する。
先ず、図２に示すように、ロボットはワークをつかみ、制御装置のインピーダンス制御処理部がロボットの手先が外力に対して柔軟に動作するようにインピーダンス制御している状態で、組み立て動作処理部があらかじめ教示された穴の上部に位置決めし（Ｓ１０１）、ワークを下げて対象物へ押し付ける（Ｓ１０２）。
次に、力・モーメントセンサから得られたＸまわりのモーメントがＭｘ、ＹまわりのモーメントがＭｙであったとき、穴中心方向をツール座標系のＸ、Ｙ成分でＰｃｄｉｒとして下記（１）のように表し（Ｓ１０３）、
【数１】
2009061550.tif 000003
計算したＰｃｄｉｒを探索方向として移動をし（Ｓ１０４）、穴中心へ近づくと押し付け力によってワークを穴へ挿入することができるので、制御装置はワークの垂直方向の位置が穴の深さ程度変化したことか、あるいは垂直方向の力が変化したことによって挿入方向に動いたことを判断し（Ｓ１０５）、ワークを挿入方向へ動かして穴底面まで移動し（Ｓ１０６）、グリッパを開いてワークから離れて動作を終了する（Ｓ１０７）。
【実施例２】
【００１０】
図３は本発明の実施例２に係る組み立て作業ロボットの制御の探索動作を示す図である。
探索動作の移動をしながら第１実施例における探索方向Ｐｃｄｉｒの計算を逐次実行し、探索方向を修正すると、図３のように、探索開始時の探索方向と、実際の対象物の中心方向との間に誤差があっても、移動しながら探索方向を修正し、対象物の中心へ向かうように動作することで３０１のような経路をたどる。
本発明が従来技術と異なる部分は、実施例１の力・モーメントセンサから得られたモーメントを利用して探索方向の決定をする処理を探索動作中にも実行する部分（例えば、Ｓ４０１〜Ｓ４０７の処理、特にＳ４０２〜Ｓ４０５の処理で、制御装置の組み立て動作処理部に相当する）である。
【００１１】
つぎに動作について説明する。
図４は図３に示す探索動作のフローチャートである。
図４に示すように、まず、ロボットはワークをつかみ、制御装置のインピーダンス制御処理部がロボットの手先が外力に対して柔軟に動作するようにインピーダンス制御している状態で、組み立て動作処理部があらかじめ教示された穴の上部に位置決めし（Ｓ４０１）、ワークを下げて対象物へ押し付ける（Ｓ４０２）。次に、力・モーメントセンサから得られたＸまわりのモーメントがＭｘ、ＹまわりのモーメントがＭｙであったとき、穴中心方向をＰｃｄｉｒとして表し（Ｓ４０３）、計算したＰｃｄｉｒを探索方向として移動をし（Ｓ４０４）、ワークの垂直方向の位置が穴の深さ程度変化するか、あるいは垂直方向の力が変化する挿入条件を満たしたかどうか判断し（Ｓ４０５）、Ｓ４０５を満たすまで、移動しながら前記Ｓ４０３からＳ４０５を繰り返し、前記挿入条件を満たしたと判断したら、ワークを挿入方向へ動かして穴底面まで移動し（Ｓ４０６）、グリッパを開いてワークから離れて動作を終了する（Ｓ４０７）。
【実施例３】
【００１２】
図５は実施例３に係る組み立て作業ロボットの制御の探索動作を示す図である。
本発明が従来技術と異なる部分は、制御装置の組み立て動作処理部（例えば、Ｓ６０１〜Ｓ６０９の処理）の挿入位置探索処理に、Ｘ、Ｙに振動させながらＺまわりに回転させる処理を備えた部分である。ワーク中心は５０１のような経路をたどる。探索範囲は、従来技術のように挿入可能範囲をもとにして決めることはせず、探索方向成分の速度の順位もない。
【００１３】
つぎに動作について説明する。
図６は図５に示す処理のフローチャートであり、まず、ロボットはワークをつかみ、制御装置のインピーダンス制御処理部がロボットの手先が外力に対して柔軟に動作するようにインピーダンス制御している状態で、組み立て動作処理部があらかじめ教示された穴の上部に位置決めし（Ｓ６０１）、ワークを下げて対象物へ押し付ける（Ｓ６０２）。次に、力・モーメントセンサから得られたＸまわりのモーメントがＭｘ、ＹまわりのモーメントがＭｙであったとき、穴中心方向をＰｃｄｉｒで表し（Ｓ６０３）、計算したＰｃｄｉｒを探索方向として移動をし（Ｓ６０４）、Ｘ、Ｙともに、あらかじめ決められた探索範囲の境界へ達した場合には進行方向を折り返し（Ｓ６０５）、Ｘ、Ｙともにあらかじめ決められた回数だけ探索範囲を往復したら、Ｚまわりにあらかじめ決められた角度だけ回転させ（Ｓ６０６）、ワークの垂直方向の位置が穴の深さ程度変化するか、あるいは垂直方向の力が変化する挿入条件を満たしたかどうか判断し（Ｓ６０７）、Ｓ６０７を満たすまで、前記Ｓ６０３からＳ６０７を繰り返し、前記挿入条件を満たしたと判断したら、ワークを挿入方向へ動かして穴底面まで移動し（Ｓ６０８）、グリッパを開いてワークから離れて動作を終了する（Ｓ６０９）。
【実施例４】
【００１４】
図７は実施例４に係る組み立て作業ロボットの制御の探索動作を示す図である。
図７中、７０１はワーク、７０２は対象物、７０３は対象物側コネクタ、７０４はＸ、Ｙ探索方向、７０１ｈはワーク７０１にあけられ対象物コネクタ７０３が挿入されるコネクタ挿入穴である。本発明が従来技術と異なる部分は、制御装置の組み立て動作処理部（例えば、Ｓ９０１〜Ｓ９１０の処理相当）に、ワークを傾けてＸ、Ｙに振動させる処理を備えた部分である。従来技術のように中心位置を求めるスキャンのためではなく、傾けて振動させる動作は、嵌め込み動作として行う。
本実施例では、図８のようにワークおよび対象物はコネクタの形状であるとする。また、探索を開始する最初の押し付け位置がコネクタ部分にかかるように、初期位置を教示しているものとする。挿入可能な位置ではない位置での凹凸による引っかかりを避けるため、振動方向に対して図７のようにワークを傾け、傾けたままＸ、Ｙ方向７０４へ振動させ、さらに図８のようにＺまわり回転方向８０１に振動させる動作をする。ロボットは、押し付け開始から常に挿入方向へ力をかけているので、挿入可能な位置へ来ると自動的に押し付け力によってコネクタが入る。
【００１５】
つぎに動作について説明する。
図９は図７、８に示す処理のフローチャートであり、まず、ロボットはワークをつかみ、制御装置のインピーダンス制御処理部がロボットの手先が外力に対して柔軟に動作するようにインピーダンス制御している状態で、組み立て動作処理部があらかじめ教示された穴の上部に位置決めし（Ｓ９０１）、ワークを下げて対象物へ押し付ける（Ｓ９０２）。次に、力・モーメントセンサから得られたＸまわりのモーメントがＭｘ、ＹまわりのモーメントがＭｙであったとき、対象コネクタ中心方向をツール座標系のＸ、Ｙ成分でＰｖｄｉｒとして下記（２）のように表し（Ｓ９０３）、
【数２】
2009061550.tif 000004
探索方向をＰｖｄｉｒとして、探索方向に対して、あらかじめ決められた角度だ
けワークを傾け（Ｓ９０４）、計算したＰｖｄｉｒを探索方向として移動をし（Ｓ９０５）、Ｘ、Ｙともに、あらかじめ決められた探索範囲の境界へ達した場合には進行方向を折り返し（Ｓ９０６）、Ｘ、Ｙともにあらかじめ決められた回数だけ探索範囲を往復したら、Ｚまわりにあらかじめ決められた角度だけ回転させ（Ｓ９０７）、ワークの垂直方向の位置が変化するか、あるいは垂直方向の力が変化する挿入条件を満たしたかどうか判断し（Ｓ９０８）、Ｓ９０８を満たすまで前記Ｓ９０３からＳ９０８を繰り返し、前記挿入条件を満たしたと判断したら、ワークを挿入方向へ動かして止まるまで移動し（Ｓ９０９）、グリッパを開いてワークから離れて動作を終了する（Ｓ９１０）。
【００１６】
このように、探索動作においてモーメントをもとに挿入する中心方向を求めて動作し、平面上で振動させるとともに振動方向を回転させて探索する動作をするという手順をとるので、挿入可能な位置にワーク位置が接近し、押し付け力によってワークを対象物の挿入位置へ挿入することができる。
【図面の簡単な説明】
【００１７】
【図２】本発明の実施例１に係る組み立て作業ロボットの制御装置のフローチャートである。
【図１】図２に示す作業ロボットの探索動作を示す図である。
【図３】本発明の実施例２に係る組み立て作業ロボットの制御の探索動作を示す図である。
【図４】図３示す探索動作のフローチャートである。
【図５】本発明の実施例３に係る組み立て作業ロボットの制御の探索動作を示す図である。
【図６】図５に示す探索動作のフローチャートである。
【図７】本発明の実施例４に係る組み立て作業ロボットの制御の探索動作を示す図である。
【図８】図７に示すワークのコネクタの探索動作の説明図である。
【図９】図７に示す探索動作のフローチャートである。
【図１０】従来の組み立てロボットの制御装置の配置図である。
【図１１】従来の組み立てロボットの制御装置のブロック図である。
【図１２】従来の組立て方法及び装置の探索動作の説明図である。
【図１３】従来のピン挿入方法及び装置の側面図である。
【図１４】四角柱の場合の従来のピン挿入方法を説明する説明図である。
【符号の説明】
【００１８】
２０１  対象物
２０２  対象物の穴
２０３  対象物の穴の中心
２０４  対象物の挿入可能範囲
２０５  ワーク
２０６  ワーク中心
２０７  穴の中心方向
２０８  ツール座標系
２０９  Ｘまわりモーメント
２１０  Ｙまわりモーメント
３０１  探索方向Ｐｃｄｉｒ
５０１  探索経路
７０１  ワーク
７０１ｈ  ワークのコネクタ挿入穴
７０２  対象物
７０３  対象物側コネクタ
７０４  Ｘ、Ｙ探索方向
８０１  Ｚまわり探索方向
【図１】
2009061550.tif 000005
【図２】
2009061550.tif 000006
【図３】
2009061550.tif 000007
【図４】
2009061550.tif 000008
【図５】
2009061550.tif 000009
【図６】
2009061550.tif 000010
【図７】
2009061550.tif 000011
【図８】
2009061550.tif 000012
【図９】
2009061550.tif 000013
【図１０】
2009061550.tif 000014
【図１１】
2009061550.tif 000015
【図１２】
2009061550.tif 000016
【図１３】
2009061550.tif 000017
【図１４】
2009061550.tif 000018