JPWO2019163671A1 - 線状物の先端移動方法、制御装置、および、三次元カメラ - Google Patents

Abstract

この線状物の先端移動方法は、ロボットハンド（２２）で把持した線状物（Ｗ１）の先端（Ｗ１ｓ）を目標位置（ＴＧ１）に移動させる際の、線状物の先端移動方法であって、ロボットハンド（２２）で把持した線状物（Ｗ１）の先端（Ｗ１ｓ）の位置を計測する工程と、計測した先端（Ｗ１ｓ）の位置に基づき、先端（Ｗ１ｓ）を目標位置（ＴＧ１）に移動させる工程と、を備える。

Description

本発明はロボットハンドを用いて線状物を把持した場合の、線状物の先端移動方法、制御装置、および、三次元カメラに関する。

対象物を三次元カメラ等で認識して自律的に把持するロボットの普及が進んでいる。線状物を把持することについては、たとえば特開２０１４−１７６９１７号公報（特許文献１）に、線状体の組み付け作業を行なうロボット装置であって、一端が固定された線状体の固定端近傍を把持したのち、把持部を所定の軌跡でスライドさせて他端に移動させる装置が記載されている。これにより、線状物の一例である電線に付いた癖等により正確に推定することが困難な他端を素早く把持できるとされる。

特開２０１６−１９２１３８号公報（特許文献２）には、ワイヤーハーネスの製造方法および画像処理方法に関する発明が開示され、ワイヤーハーネスを製造する過程において、電線集合体の三次元形状が測定されることによって加工位置が特定される加工位置特定処理が実施される。

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

ロボットハンドで線状物としてたとえば電線等を保持し、所定のターゲット位置に電線の先端を移動させる制御を行なう場合が考えられる。たとえば、対象物に設けられた貫通孔に、その電線の先端を挿入する制御が想定される。ロボットハンド側では、ロボットハンドで把持した領域から先端側に向っては、その線状物は真っ直ぐに延びていると認識している。

しかしながら、実際には、ロボットハンドで把持した領域から先端側に向って、線状物が曲がっている場合が想定される。そのような場合には、線状物の先端位置を認識した後に、ロボットハンド側の移動を実施して、対象物に設けられた貫通孔に、その電線の先端を移動させる必要がある。

この発明は、上記課題を解決することを目的としており、ロボットハンドで線状物を保持した場合の、線状物の先端移動方法、制御装置、および、三次元カメラに関する。

この線状物の先端移動方法においては、ロボットハンドで把持した線状物の先端を目標位置に移動させる際の、線状物の先端移動方法であって、上記ロボットハンドで把持した上記線状物の上記先端の位置を計測する工程と、計測した上記先端の位置に基づき、上記先端を上記目標位置に移動させる工程と、を備える。

他の形態においては、上記先端の位置を計測する工程は、さらに、上記先端の向きも計測する工程を含む。

他の形態においては、上記先端を上記目標位置に移動させる工程は、上記先端の向きを所定の向きに一致させる工程を含む。

他の形態においては、上記先端の向きは、上記先端から所定距離位置までの上記線状物の形状に基づいて決定する。

他の形態においては、上記先端を上記目標位置に移動させる工程は、上記目標位置が孔である。

他の形態においては、上記先端を上記目標位置に移動させる工程は、上記先端が予め定めた手前位置を通過し、上記手前位置における上記先端の向きを所定の向きに一致させて、上記先端を移動させる。

この制御装置においては、ロボットに設けられたロボットハンドによる線状物の移動を制御する制御装置であって、上記線状物の三次元形状を取得する三次元カメラから上記線状物の三次元形状を取得し、上記三次元形状から上記線状物の先端の位置を取得し、上記線状物の上記先端の位置に基づき、上記先端を目標位置に移動させるための情報を上記ロボットハンドを有する上記ロボットに通知する。

他の形態においては、当該制御装置は、上記線状物の上記三次元形状から上記線状物の上記先端の向きを計測し、上記線状物の上記先端の向きを所定の向きに一致させて上記目標位置まで移動させるための情報を上記ロボットハンドを有する上記ロボットに通知する。

この三次元カメラにおいては、ロボットに設けられたロボットハンドによる線状物の移動を制御する際に用いられる、上記線状物の三次元形状を取得する三次元カメラであって、当該三次元カメラは、制御装置を含み、上記制御装置は、上記三次元カメラから上記線状物の三次元形状を取得し、上記三次元形状から上記線状物の先端の位置を取得し、上記線状物の上記先端の位置に基づき、上記先端を目標位置に移動させるための情報を上記ロボットハンドを有する上記ロボットに通知する。

この線状物の先端移動方法、制御装置、および、三次元カメラによれば、ロボットハンドで把持した領域から先端側に向って、線状物が曲がっている場合であっても、線状物の先端位置を認識した後に、ロボットハンド側の移動を実施して、対象物に設けられた貫通孔に、その電線の先端を移動させることを可能とする。

関連技術における線状物把持方法を実行する全体システムを示す図である。 関連技術における線状物把持方法の工程フロー図である。 関連技術における線状物把持方法の判定工程の工程フロー図である。 関連技術における第１干渉領域および第１拡張干渉領域を示す図である。 関連技術における第２干渉領域および第２拡張干渉領域を示す図である。 関連技術における第１干渉領域の大きさを説明するための図である。 関連技術における第１判定工程を説明するための図である。 関連技術における第１予備判定工程を説明するための図である。 実施の形態における線状物の先端移動方法を説明する概略図である。 実施の形態における線状物の他の先端移動方法を説明する概略図である。

本発明に基づいた実施の形態の先端曲り量計測方法について、以下、図を参照しながら説明する。以下に説明する実施の形態において、個数、量などに言及する場合、特に記載がある場合を除き、本発明の範囲は必ずしもその個数、量などに限定されない。同一の部品、相当部品に対しては、同一の参照番号を付し、重複する説明は繰り返さない場合がある。実施の形態における構成を適宜組み合わせて用いることは当初から予定されていることである。図においては、実際の寸法比率では記載しておらず、構造の理解を容易にするために、一部比率を異ならせて記載している。

以下の説明においては、線状物の一例として電線を用いた場合について説明しているが、電線に限定されるものではない。この説明での線状物とは、細長い形状を有する物体であれば何でもよい。線状物の一例としては、電線、ワイヤーハーネス、はんだ、紐、糸、繊維、ガラス繊維、光ファイバ、チューブ、乾麺等が挙げられる。細線を束にした電線に限定されず、一本線から構成される電線等も含まれる。特に、たわみが生じるなどして形状が変化する線状物や、直線ではない線状物の場合、本実施の形態の効果がより顕著に表れる。

（関連技術：線状物把持方法および制御装置）
以下、図１から図８を参照して、関連技術として、線状物把持方法および制御装置の一例について説明する。

図１において、線状物把持方法を実施するための全体システム１０は、ロボット２０と、三次元カメラ３１と、制御装置３２とを有する。作業空間には、電線Ｗ１、電線Ｗ２、電線Ｗ３から構成されるワイヤーハーネスＷが配置されている。

ロボット２０としては、公知の多関節ロボットを好適に利用することができる。ロボットのアーム２１の先端にはロボットハンド２２が備えられており、ロボットハンドの一対の把持部２３、２３で線状物を把持する。

三次元カメラ３１は、電線Ｗ１、電線Ｗ２、電線Ｗ３の三次元形状を計測できるものであれば特に限定されない。好ましくはステレオカメラを用いる。ステレオカメラは線状物の三次元形状を高速に計測するのに好ましい。

ステレオカメラは２台のカメラを含み、異なる視点から撮像された２枚の画像上で計測したい点の対応点を求め、２台のカメラの位置関係から３角測量の原理によって計測点の三次元位置を算出する。ステレオ方式による線状物の三次元計測に関しては、たとえば、特開平２−３０９２０２号公報には、多数の線状物を２台のカメラで撮像し、２つの画像中の輝線の傾きと輝線間の距離を特徴として照合することにより対応点を決定することが記載されており、これによって、対応点の決定にかかる処理時間を短縮できるとされる。

ステレオ方式において、一方の画像の視点と計測点を結ぶ直線を他方の画像上に（特願２０１７―２２１０４５号参照）投影した直線をエピポーラ線といい、一方の画像上の点に対応する他方の画像上の対応点は必ず他方の画像上のエピポーラ線上に投影されている。

このことを利用して、線状物の上のある点の対応点を求めるには、他方の画像上で線状物とエピポーラ線の交点を求めればよく、高速に線状物の三次元形状を計測できる。線状物が互いに異なる色に色分けされている場合には、カラーカメラを用いることにより、画像から該当する色を抽出してから対応点を求めることで、各線状物の三次元形状をより高速に求めることができる。

制御装置３２は、図示しない通信部によって三次元カメラ３１と通信し、ステレオカメラから電線Ｗ１、電線Ｗ２、電線Ｗ３の三次元形状を取得する。制御装置は図示しない演算部によって、ステレオカメラから取得した三次元形状に基づいて、ロボットハンド２２が線状物を把持する際に他の線状物と干渉するか否かを判定し、把持すべき目標線状物を決定するための各種演算を行なう。

制御装置は上記通信部を介して、演算結果に基づいて、把持すべき目標線状物の把持位置をロボット２０に通知する。把持位置をロボット２０に直接通知するだけでなく、制御装置３２とロボット２０との間にロボットの動作を制御する別の装置（たとえば、ロボットコントローラや制御用パソコン等）を設け、それらの装置に対して通知しても良い。

図２を参照して、本関連技術の線状物把持方法を以下に説明する。この関連技術の線状物把持方法は、複数の電線Ｗ１、電線Ｗ２、電線Ｗ３の三次元形状を計測する工程（Ｓ１）、計測された三次元形状に基づいてロボットハンド２２が線状物を把持する際に他の線状物が干渉するか否かを判定する判定工程（Ｓ２）、判定工程Ｓ２における判定結果に基づいて決定された目標線状物を把持する工程（Ｓ３）を含む。

複数の電線Ｗ１、電線Ｗ２、電線Ｗ３の三次元形状を計測する工程Ｓ１は三次元カメラ３１によって実施される。ステレオカメラは線状物のある作業空間を撮像し、２枚の画像を演算処理して電線Ｗ１、電線Ｗ２、電線Ｗ３のそれぞれの三次元形状を取得する。線状物の三次元形状は、直交座標系または斜交座標系で表され、好ましくは直交座標で表される。

判定工程Ｓ２は制御装置３２によって実施される。判定工程については詳細を後述する。

目標線状物を把持する工程Ｓ３はロボット２０によって実施される。ロボットは、把持すべき目標線状物の把持位置を制御装置３２から通知され、ロボットアーム２１およびロボットハンド２２を移動させて把持動作を実行する。

以下に判定工程Ｓ２を詳細に説明する。
図３を参照して、本関連技術の判定工程Ｓ２では、線状物の三次元形状の取得（Ｓ２１）、注目線状物の選択（Ｓ２２）、注目線状物の把持位置の決定（Ｓ２３）、ロボットハンド待機位置の取得（Ｓ２４）、各種干渉領域の設定（Ｓ５１〜Ｓ５４）と各種干渉判定（Ｓ６１〜Ｓ６４）を実施する。

制御装置３２はまず、三次元カメラ３１から電線Ｗ１、電線Ｗ２、電線Ｗ３の三次元形状を取得する（Ｓ２１）。

次に、制御装置３２は、ロボットハンド２２で把持しようとする注目線状物を選択する（Ｓ２２）。以下において、電線Ｗ１を把持しようとする注目線状物、電線Ｗ２と電線Ｗ３を注目線状物以外の線状物（他の線状物）として説明する。制御装置は、電線の色などの指定を外部から受けて、その指示に基づいて注目線状物を決定してもよい。好ましくは、制御装置は自律的（特願２０１７―２２１０４５号参照）に注目線状物を選択する。

たとえば、電線Ｗ１、電線Ｗ２、電線Ｗ３が台の上に置かれている場合、取得した三次元形状に基づいて最も高い位置、すなわち最も上にある線状物を注目線状物として選択することができる。線状物が重なり合って置かれている場合でも、上にある線状物ほど、その線状物を把持するときに他の線状物が干渉する確率が低いからである。

次に、制御装置３２は、注目すべき電線Ｗ１の把持位置を決定する（Ｓ２３）。たとえば、注目線状物の先端から何ｍｍというような予め定められた条件に基づいて、制御装置が注目線状物の把持位置を三次元座標として算出する。

次に、制御装置３２は、ロボットハンド２２の待機位置を取得する（Ｓ２４）。ロボットハンド２２の待機位置が予め定められている場合は、その座標を待機位置として取得する。待機位置を線状物の三次元形状に基づいて決定する場合、たとえば線状物から所定距離離れた上方に決定するなどの場合は、演算により待機位置を取得する。

制御装置３２はロボットハンド２２の現在位置をロボット２０から取得し、ロボットハンド２２の現在位置が待機位置と異なる場合は、ロボットハンド２２を待機位置に移動させる。ロボットハンド２２の待機位置と電線Ｗ１の把持位置を結ぶ線分が、ロボットハンド２２が把持動作を実行するときのおおよその移動経路を与える。

次に、制御装置３２は、ロボットハンド２２と他の電線Ｗ２、電線Ｗ３との干渉判定のために、注目線状物の把持位置を含むいくつかの干渉領域を設定する。図３では、第１干渉領域、第１拡張干渉領域、第２干渉領域、第２拡張干渉領域の順に設定している。すべての他の線状物の１本毎に、当該他の線状物が上記それぞれの干渉領域に含まれるか否かを判定する干渉判定を行なう。

各線状物についての干渉判定は、線状物上の点または線分を長さ方向にずらしながら、その点が干渉領域内にあるかまたはその線分が干渉領域と交差するかを判定することによって行なうことができる。図３では、第２拡張干渉領域に対する第２予備判定、第２干渉領域に対する第２判定、第１拡張干渉領域に対する第１予備判定、第１干渉領域に対する第１判定の順に実施している。以下に、図３の順番とは異なるが、各干渉領域とその領域に対する干渉判定とについて説明する。

図４を参照して、第１干渉領域５１に対する第１判定工程Ｓ６１は、ロボットハンド２２が電線Ｗ１を把持するときに他の電線Ｗ２、Ｗ３と干渉するか否かを判定する。

第１干渉領域５１は、電線Ｗ１の把持位置Ｐを含み、所定の形状および所定の大きさを有する平面状の領域である。第１干渉領域は、その中心に把持位置Ｐを含むことが好ましい。第１干渉領域の形状は特に限定されないが、好ましくは、多角形、円または楕円とする。第１干渉領域が多角形の場合は、好ましくは４角形、より好ましくは正方形である。計算の負荷が軽くなり、高速な判定が可能となるからである。第１干渉領域が多角形の場合、線状物の三次元形状を表す座標系（以下、単に「座標系」という）のいずれか２本の軸がなす平面に平行な辺を有する正方形を第１干渉領域とするのが特に好ましい。

後述する第１拡張干渉領域をより小さくして第１予備判定の効率を向上できるからである。第１干渉領域が多角形でない場合は、好ましくは円である。同じく、計算の負荷が軽くなり、高速な判定が可能となるからである。

第１干渉領域５１の大きさは、大きすぎると実際には干渉しないのに干渉すると誤判定する確率が増大する。図６を参照して、ロボットハンド２２の最大断面に外接する最小の円を円Ｃ１とすると、第１干渉領域は、好ましくは、円Ｃ１の２．０倍の直径を有する円に内包される大きさであり、より好ましくは、円Ｃ１と同じ大きさの円に内包される大きさである。

一方、第１干渉領域が小さすぎると、実施には干渉するのに干渉しないと誤判定する確率が増大する。図６を参照して、ロボットハンド２２が線状物を把持するために動作させる把持部２３の最大断面に外接する最小の円を円Ｃ２とすると、第１干渉領域は、好ましくは、円Ｃ２（特願２０１７―２２１０４５号参照）と同じ大きさの円を内包できる大きさである。

第１干渉領域５１は、好ましくは、電線Ｗ１と直交する。第１干渉領域が注目線状物と直交するとは、把持位置Ｐにおいて注目線状物が伸びる方向が第１干渉領域と直角をなすことをいう。第１干渉領域を含む平面の方程式が容易に求められるからである。また、ロボットハンド２２で線状物を把持する場合、線状物を真横から、つまり線状物と直角の方向から把持することが多いからである。注目線状物と直交する第１干渉領域内に他の線状物が存在する場合には、ロボットハンド２２が注目線状物を真横から把持しない場合であっても、ロボットハンド２２が当該他の線状物と干渉する蓋然性が高いからである。

図７を参照して、第１判定工程Ｓ６１は、対象とする他の電線Ｗ２上の線分Ｌと第１干渉領域５１との交差判定によって行なうことができる。線分Ｌは、電線Ｗ２の三次元形状を表す点群のうち隣り合う２点Ｓ、Ｔ間の線分とすることができる。線分Ｌが第１干渉領域と交差するなら、線分Ｌ上のどこかの点が第１干渉領域に含まれる。交差判定は公知の方法で行なうことができる。たとえば、第１干渉領域５１を含む平面Ｕの法線ベクトルＮと、把持位置Ｐから線分Ｌの両端Ｓ、ＴへのベクトルＰＳおよびＰＴとの内積を取り、２つの内積の符号が異なる場合は線分Ｌは平面Ｕと交差する。線分Ｌと平面Ｕが交差する場合は、その交点が第１干渉領域５１内にあるか否かを判定すればよい。

図４を参照して、第１拡張干渉領域５２に対する第１予備判定工程Ｓ６２は、第１判定に先だって実施され、ロボットハンド２２と他の電線Ｗ２、Ｗ３が干渉しない場合を、より高速な計算で発見するために行なう。

第１拡張干渉領域５２は第１干渉領域５１を内包する空間領域である。第１拡張干渉領域の形状や大きさは特に限定されないが、好ましくは、第１干渉領域を内包し、すべての辺が座標系のいずれかの軸に平行な６面体のうち最小のものを第１拡張干渉領域として設定する。座標系が直交座標系の場合は、この６面体は直方体である。これにより、座標の大小比較を行なうだけで、第１予備判定が実施できる。

具体的には、図８を参照して、第１拡張干渉領域５２の８つの頂点Ａ〜Ｈの座標を図８のとおりとし、線分Ｌの一方の端点Ｓの座標を（ｘＳ，ｙＳ，ｚＳ）とすると、ｘ１≦ｘＳ≦ｘ２、かつ、ｙ１≦ｙＳ≦ｙ２、かつ、ｚ１≦ｚＳ≦ｚ２、であれば点Ｓは第１拡張干渉領域内にあり、そうでなければ点Ｓは第１拡張干渉領域外にある。

第１拡張干渉領域５２が第１干渉領域５１を内包するので、第１予備判定によってハンドと他の線状物が干渉しないとの結果が得られた場合は、第１判定を省略できる。

図５を参照して、第２干渉領域５３に対する第２判定工程（Ｓ６３）は、ロボットハンド２２が電線Ｗ１の把持位置Ｐまで移動する経路で、他の電線Ｗ２、電線Ｗ３と干渉するか否かを判定する。

第２干渉領域５３は、電線Ｗ１の把持位置Ｐとロボットハンド２２の待機位置Ｑとを結ぶ線分ＰＱを含み、線分ＰＱの両側に広がり所定の幅を有する平面状の領域である。第２干渉領域は、その幅方向の中心に線分ＰＱを含むことが好ましい。第２干渉領域の形状は、特に限定されないが、好ましくは長方形または平行四辺形であり、より好ましくは、線分ＰＱを線対称の対称軸とする長方形である。計算の負荷を軽くして、より高速に判定するためである。

第２干渉領域５３の幅は、広すぎると実際には干渉しないのに干渉すると誤判定する確率が増大する。第２干渉領域の幅は、好ましくは、図６の円Ｃ１の直径以下である。一方、第２干渉領域の幅が狭すぎると、実施には干渉するのに干渉しないと誤判定する確率が（特願２０１７―２２１０４５号参照）増大する。第２干渉領域の幅は、好ましくは、図６の円Ｃ２の直径以上である。

第２干渉領域５３は、好ましくは、電線Ｗ１との交角が最大となるように設定される。ロボットハンド２２が電線Ｗ１に接近する際に、把持部２３、２３がそのような平面内を進むことが多いからである。

第２判定工程Ｓ６３は、第１判定工程Ｓ６１と同様に、対象とする他の電線Ｗ２上の線分Ｌと第２干渉領域５３との交差判定によって行なうことができる。

第２拡張干渉領域５４に対する第２予備判定工程（Ｓ６４）は、第２判定に先だって実施され、ロボットハンド２２と他の電線Ｗ２、電線Ｗ３が干渉しない場合を、より高速な計算で発見するために行なう。

第２拡張干渉領域５４は第２干渉領域５３を内包する空間領域である。第２拡張干渉領域の形状や大きさは特に限定されないが、好ましくは、第２干渉領域を内包し、すべての辺が座標系のいずれかの軸に平行な６面体のうち最小のものを第２拡張干渉領域として設定する。座標系が直交座標系の場合は、この６面体は直方体である。これにより、座標の大小比較を行なうだけで、第２予備判定が実施できる。

第２拡張干渉領域５４が第２干渉領域５３を内包するので、第２予備判定によってハンドと他の線状物が干渉しないとの結果が得られた場合は、第２判定を省略できる。

判定の対象とする線分Ｌを電線Ｗ２の長さ方向にずらしながら上記判定工程Ｓ６１〜Ｓ６４を繰り返して、他の電線Ｗ２との干渉判定が完了したら、次の他の電線Ｗ３について同じ処理を行なう。

すべての他の電線Ｗ２、Ｗ３が干渉領域５１〜５４に含まれないと判定された場合は、制御装置３２は電線Ｗ１の把持位置をロボットハンド２２で把持する際に他の線状物の干渉がないと判定する。その後、電線Ｗ１を目標線状物として、その把持位置をロボット２０に通知する。

いずれかの第１判定または第２判定で線分Ｌが第１干渉領域または第２干渉領域に含まれると判定された場合は、制御装置３２は電線Ｗ１の把持位置をロボットハンド２２で把持する際に他の線状物の干渉があると判定する。以後の判定工程を省略して工程Ｓ２２に戻り、注目線状物を変えて同じ処理を繰り返す。制御装置３２が自律的に次の注目線状物を選択する場合は、たとえば、先に三次元カメラ３１から取得した電線Ｗ１〜Ｗ３の三次元形状に基づいて、次に高い位置にある線状物を注目線状物として選択することができる。

何れの線状物に注目しても他の線状物と「干渉あり」と判定された場合は、線状物全体を回転させて向きを変えたり、線状物を振ったり振動させたりして線状物同士の位置関係を変化させてから、再度各工程を実施してもよい。各注目線状物の把持位置から最も近い他の線状物までの距離を干渉距離として計算しておき、干渉距離の長い線状物から把持するようにしてもよい。これにより、把持が成功しやすい順番で把持動作を実行するようロボットに指示することができる。干渉距離は、干渉判定における第１干渉領域または第２干渉領域と他の線状物との交点から把持位置までの距離を用いることで簡易に計算することができる。

以上のとおり、本関連技術の線状物把持方法によれば、線状物と他の線状物が干渉するか否かの判定結果に基づいて把持動作を実行するので、複数の線状物から１本の線状物を（特願２０１７―２２１０４５号参照）選んでロボットハンド２２で把持することが可能となる。

ロボットハンド２２の他の線状物との干渉の有無は、ロボットハンド２２側のＣＡＤデータと線状物の三次元形状データを用いて、多面体との交差の有無を計算することによって実施してもよい。しかしこの方法は判定の正確さにおいて優れるが、時間のかかる処理である。

この関連技術では、注目線状物以外の線状物が第１干渉領域内に存在するか否かを、平面状の第１干渉領域と線状物との交差判定によって判定できるので、計算量が少なく、干渉の有無を高速に判定できる。第１干渉領域内に注目線状物以外の線状物が存在しない場合は、ロボットハンド２２が他の線状物と干渉しないで注目線状物を把持できる蓋然性が高い。第２干渉領域についても同様である。

各判定工程を実施する順番は、第１予備判定を第１判定に先立って行ない、第２予備判定を第２判定に先立って行なう以外は、特に限定されない。上記実施形態では、第２判定工程を先に、第１判定工程を後で実施したが、この順番を逆にしてもよい。上記関連技術では、線分Ｌを線状物の長さ方向にずらしながら、１つの線分毎にすべての判定工程を実施したが、ある線状物について一つの判定工程（たとえば第２予備判定工程）を終えてから、改めて同じ線状物について他の判定工程（たとえば第２判定工程）を実施してもよい。

上記関連技術では、ロボットハンド２２の待機位置取得（Ｓ２４）に先立って注目線状物を選択したが（Ｓ２２）、先にロボットハンド２２の待機位置を取得し、その待機位置に基づいて注目線状物を選択してもよい。その場合は、注目線状物として最も待機位置側にある線状物を選択できる。最も待機位置側にある線状物として、待機位置の座標と当該線状物の把持位置の座標との距離が最も短い線状物を選択してもよい。これにより、把持する際に他の線状物と干渉する可能性の低い線状物を優先的に選択できる点で好ましい。

ロボットハンド２２が線状物を把持する際の姿勢（把持姿勢）は、把持部と線状物が略直角をなすように把持することが好ましい。把持部に対して把持位置から先端側の線状物の向きが略垂直であれば、把持した後に加工機等に挿入する際もロボットの制御が容易になるからである。好ましくは、待機位置において、把持した際に把持部と線状物とが直角をなす向きになるように、ロボットハンド２２の姿勢を調整する。その後、ロボットハンド２２は、待機位置から把持位置に向かって第２干渉領域に沿って移動する。これにより、ロボットハンド２２の姿勢、ロボットハンド２２の移動方向、ならびに第１および第２干渉領域の平面の向きが一致するため、高精度な干渉判定が可能となる。

本発明によって線状物を把持したロボットハンド２２が当該線状物を種々の製造装置・加工装置まで搬送してもよい。たとえば、把持した電線の先端をロボットハンド２２によって移動させ、被膜剥き加工機や端子圧着装置等に挿入してもよい。電線の先端をコネクタ等の各種部品に挿入しワイヤーハーネスを製造する工程に用いてもよい。

（実施の形態：線状物の先端移動方法、制御装置、および、三次元カメラ）
次に、上記のように説明した、ロボット２０に設けられたロボットハンド２２が当該線状物を種々の製造装置・加工装置まで搬送する場合について検討する。具体的には、制御装置３２（図１参照）により、ロボット２０に設けられたロボットハンド２２による線状物Ｗ１の移動を制御する。制御装置３２は、線状物Ｗ１の三次元形状を取得する三次元カメラ３１から線状物Ｗ１の三次元形状を取得し、この三次元形状から線状物Ｗ１の先端の位置を取得し、線状物Ｗ１の先端Ｗ１ｓの位置に基づき、先端Ｗ１ｓを目標位置に移動させるための情報をロボットハンド２２を有するロボット２０に通知することを想定している。

図１に示した全体システム１０においては、ロボットハンド２２を有するロボット２０、三次元カメラ３１、および、制御装置３２を有する構成としている。この制御装置３２は、ロボット２０および三次元カメラ３１から独立した制御装置、ロボット２０が備える制御装置、および、三次元カメラ３１が備える制御装置のいずれであってもよい。

上記したように、ロボットハンド２２が線状物を把持する際の姿勢（把持姿勢）は、把持部と線状物が略直角をなすように把持することが好ましい。しかしながら、ある程度の柔軟性を有する線状物を用いた場合には、ロボットハンド２２で把持した線状物の先端側が真っ直ぐに延びておらず、曲がっている場合が想定される。

このような場合に、ロボットハンド２２側が、線状物の先端側の曲りを認識しないまま線状物を搬送した場合には、所定の目標位置に線状物の先端を搬送できない状況となる。

そこで、制御装置３２は、線状物の三次元形状から線状物の先端の向きを計測し、線状物の先端の向きを所定の向きに一致させて目標位置まで移動させるための情報をロボットハンドを有するロボットに通知するようにするとよい。

以下の実施の形態においては、線状物を把持したロボットハンド２２による線状物を所定位置に搬送する場合の、線状物の先端曲り量計測方法について以下説明する。以下の説明では、所定位置への搬送の一例として、ターゲットＴＧに設けられた孔ＴＧＨに、線状物として電線Ｗ１の先端Ｗ１ｓを挿入させる場合について説明する。

ここで、線状物の直径は、三次元カメラで認識できる範囲であればよく、好ましくは０．０１ｍｍ〜１０ｃｍである。ターゲットＴＧに設けられた孔ＴＧＨは、線状物を挿入することができる大きさであればよく、好ましくは直径が０．０１ｍｍ〜１５ｃｍ、孔深さは線状物の直径の２倍以上が好ましい。

図９を参照して、ロボットハンド２２により電線Ｗ１を把持した状態を示している。さらに電線Ｗ１の先端Ｗ１ｓは、ロボットハンド２２に対して略直角をなすように延びておらず曲がった状態となっている。

ロボットハンド２２により把持された電線Ｗ１の先端Ｗ１ｓの位置を計測する。この時、ロボットハンド２２に対して略直角に延びる方向をＬ１とした場合に、電線Ｗ１の先端Ｗ１ｓがＬ１に対してどれだけずれているかの先端曲り量を計測してもよい。先端Ｗ１ｓの位置の計測においては、上述した三次元カメラ３１を用いて三次元形状を計測できる。これにより、先端Ｗ１ｓの位置やズレ量Ｘ１を知ることができる。

さらに、電線Ｗ１の先端Ｗ１ｓから所定距離位置（図９中の距離Ｄ１）までの形状を測定し、平均ベクトルを求めることで、電線Ｗ１の先端の延びる向きを決定することができる。また、電線Ｗ１の先端Ｗ１ｓと、先端Ｗ１ｓから孔への挿入長さの位置とを結ぶベクトルを先端の向きとしてもよい。

これにより、ターゲットＴＧに設けられた孔ＴＧＨに先端Ｗ１ｓを挿入する際の適切な姿勢の向きが判定できる。電線Ｗ１の先端Ｗ１ｓから所定距離位置（図９中の距離Ｄ１）は、好ましくはターゲットＴＧに設けられた孔ＴＧＨの孔深さの０．３〜１０倍、より好ましくは、０．５〜５倍である。先端からの曲り量を計測し、急峻に変化する点までをＤ１としてもよい。次に、電線Ｗ１の先端Ｗ１ｓの位置や先端曲り量の計測を終えると、その位置や先端曲り量をロボットハンド２２の移動制御にフィードバックする。

図９に示すように、先端Ｗ１ｓの位置や先端曲り量からフィードバック量を与えることで、先端Ｗ１ｓをターゲットＴＧに設けられた孔ＴＧＨに移動させることが可能である。この場合、孔ＴＧＨの孔深さが浅い場合には問題が生じることは少ない。しかし、孔ＴＧＨの孔深さが深い場合や孔ＴＧＨの直径が小さく電線Ｗ１の直径とあまり変わりない場合には、電線を孔に挿入できるような所定の向きに先端Ｗ１ｓの向きを一致させて移動させる必要がある。具体的には、孔ＴＧＨの中心軸ＴＧ−Ａの延びる方向に沿って先端Ｗ１ｓを移動させる必要がある。

そこで、図１０に示すように、孔ＴＧＨの中心軸ＴＧ−Ａの延長線上に位置する手前位置Ｐ１１を予め定めておき、手前位置Ｐ１１に先端Ｗ１ｓを移動させるとともに、孔ＴＧＨの中心軸ＴＧ−Ａの方向と、電線Ｗ１の先端の向きとを一致させる移動を行なう。

移動方法は、先端Ｗ１ｓを手前位置Ｐ１１に移動させながら向きを一致させてもよいし、先端Ｗ１ｓを手前位置まで平行移動させてから、手前位置で向きを一致させるように電線Ｗ１を回転させてもよい。手前位置は、目標位置付近であることが好ましく、目標位置が孔である場合は、孔の中心軸の延びる線上であることが好ましい。目標位置が溝である場合は、溝の直上であることが好ましい。目標位置が端子台である場合は、端子をつなげる位置から端子をつなげる方向に延びる線上であることが好ましい。

その後、中心軸ＴＧ−Ａの延びる線上に沿って先端Ｗ１ｓを移動させることにより、孔ＴＧＨの孔深さが深い場合や孔ＴＧＨの直径が小さい場合であっても、先端Ｗ１ｓを孔ＴＧＨの内部に挿通させることが可能となる。

なお、孔ＴＧＨに電線Ｗ１を挿入できたかどうかをセンサ等で判断し、挿入に失敗していた場合には、電線Ｗ１を手前位置Ｐ１１に戻して、再度電線Ｗ１の先端Ｗ１ｓ位置と向きを計測し、孔ＴＧＨの中心軸ＴＧ−Ａの延びる方向と電線Ｗ１の先端の向きとを合わせて、先端Ｗ１ｓを孔ＴＧＨに移動させてもよい。

具体的な、電線Ｗ１の電線先端曲り量計測方法について以下説明する。たとえば、ロボットハンド２２から長さ１００ｍｍの電線Ｗ１が出た状態と仮定する。

［目標位置のティーチング］
事前に、ロボット座標系における目標位置の座標を登録する。ロボットハンド２２に電線Ｗ１ではなく、１００ｍｍのまっすぐな棒Ｂ（図９参照）を把持させる。ロボットハンド２２は、適当な位置をつかんでいるので、棒Ｂの突出長さは１００±数ｍｍである。この棒Ｂは、電線と同一直径にする。その後、上記の棒Ｂを三次元カメラでスキャンする。

次に、ロボットハンド２２から棒Ｂの先端の相対座標が計算できる。これをたとえば、Ｐ１のように基準データとして保持する。

棒の先端を、電線Ｗ１を挿入したい孔位置まで移動させる。移動させた時の棒の先端位置が目標位置となる。目標位置は、孔の中に設定してもよいし、孔の入り口付近に設定してもよい。移動させた棒の先端のロボット座標系における座標を目標位置として登録する。これは、ロボットハンドの座標にＰ１を足すことで計算できる。この位置をＰ０と登録する。以上で目標位置のティーチングが終了する。

［電線の移動］
ロボットハンド２２に移動したい電線Ｗ１を把持させる。その後、電線Ｗ１を三次元カメラでスキャンする。

ロボットハンド２２から電線Ｗ１の先端Ｗ１ｓまでの相対座標が計算できる。これをたとえば、Ｐ２とすると、Ｐ０―Ｐ２で計算されるベクトルを移動ベクトルとしてロボットに通知し、ロボットが電線を移動させる。

これにより、先端Ｗ１ｓと孔ＴＧＨとの位置が合うこととなる。
［電線の移動＋方向変更］
ここで、Ｐ２は先端座標であることから、これに先端方向（Ｒｘ，Ｒｙ，Ｒｚ）を同時に計算すれば、上記したように、孔ＴＧＨの中心軸ＴＧ−Ａの延びる方向と、電線Ｗ１の先端の延びる方向とを一致させることが可能となる。先端Ｗ１ｓの移動方法は、上記したように予め定めた手前位置Ｐ１１まで平行移動させ、手前位置Ｐ１１で電線Ｗ１の先端の向きを孔ＴＧＨの中心軸ＴＧ−Ａの方向に一致させるよう回転し、その後、中心軸ＴＧ−Ａの延びる線上に沿って先端Ｗ１ｓを移動させてもよい。また、手前位置Ｐ１１を定めずに、電線Ｗ１の先端の延びる方向を孔ＴＧＨの中心軸ＴＧ−Ａの延びる方向に一致させるよう回転させながら、初期位置から孔ＴＧＨへ移動させてもよい。先端Ｗ１ｓが目標位置Ｐ０に到達する際、先端Ｗ１ｓの向きが所定の向きに一致するように、線状物Ｗ１の先端Ｗ１ｓを所定位置に移動させる移動方法であればよい。

線状物を所定位置へ搬送する一例として、ターゲットＴＧに設けられた孔ＴＧＨに、線状物として電線Ｗ１の先端Ｗ１ｓを挿入させる場合について説明したが、線状物および目標位置はこれに限定されない。目標位置が孔である具体例としては、電線の先端を検査する検査装置、電線の被膜を剥くワイヤーストリッパー、電線とコネクタを接続する加工機、電線の先端に圧着端子をかしめる加工装置などの所定位置が挙げられる。

その他の実施例として、例えば、基板に配線をはんだ付けする場合、配線の向きや他の部品の位置に応じて、はんだの向きを制御する必要がある。この場合、線状物をはんだとして、目標位置をはんだ付けしたい基板の所定箇所または基板の所定箇所の直上付近として本発明を利用できる。

また、端子付きの電線を端子台に移動させる場合には、端子を端子台につなげる向きに制約があるため、端子付き電線の向きを端子台につなげられる所定の向きに一致させるように移動させる必要がある。この場合、線状物は端子付きの電線、目標位置は端子台の所定箇所または所定箇所付近として本実施の形態を利用することができる。このように、目標位置は、孔に限定されず、所定の検査位置、加工作業位置、面や溝、またはこれらの直上付近の所定箇所でもよい。

今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて請求の範囲によって示され、請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

１０ 線状物把持のための全体システム、２０ ロボット、２１ ロボットアーム、２２ ロボットハンド、３１ 三次元カメラ（ステレオカメラ）、３２ 制御装置、５１ 第１干渉領域、５２ 第１拡張干渉領域、５３ 第２干渉領域、５４ 第２拡張干渉領域。

Claims (9)

1. ロボットハンドで把持した線状物の先端を目標位置に移動させる際の、線状物の先端移動方法であって、
   前記ロボットハンドで把持した前記線状物の前記先端の位置を計測する工程と、
   計測した前記先端の位置に基づき、前記先端を前記目標位置に移動させる工程と、
   を備える、線状物の先端移動方法。
2. 前記先端の位置を計測する工程は、さらに、前記先端の向きも計測する工程を含む、請求項１に記載の線状物の先端移動方法。
3. 前記先端を前記目標位置に移動させる工程は、前記先端の向きを所定の向きに一致させる工程を含む、請求項２に記載の線状物の先端移動方法。
4. 前記先端の向きは、前記先端から所定距離位置までの前記線状物の形状に基づいて決定する、請求項２または請求項３に記載の線状物の先端移動方法。
5. 前記先端を前記目標位置に移動させる工程は、前記目標位置が孔である、請求項１から請求項４のいずれかに記載の線状物の先端移動方法。
6. 前記先端を前記目標位置に移動させる工程は、前記先端が予め定めた手前位置を通過し、前記手前位置における前記先端の向きを所定の向きに一致させて、前記先端を移動させる、請求項１から請求項５のいずれかに記載の線状物の先端移動方法。
7. ロボットに設けられたロボットハンドによる線状物の移動を制御する制御装置であって、
   前記線状物の三次元形状を取得する三次元カメラから前記線状物の三次元形状を取得し、
   前記三次元形状から前記線状物の先端の位置を取得し、
   前記線状物の前記先端の位置に基づき、前記先端を目標位置に移動させるための情報を前記ロボットハンドを有する前記ロボットに通知する、制御装置。
8. 当該制御装置は、前記線状物の前記三次元形状から前記線状物の前記先端の向きを計測し、
   前記線状物の前記先端の向きを所定の向きに一致させて前記目標位置まで移動させるための情報を前記ロボットハンドを有する前記ロボットに通知する、請求項７に記載の制御装置。
9. ロボットに設けられたロボットハンドによる線状物の移動を制御する際に用いられる、前記線状物の三次元形状を取得する三次元カメラであって、
   当該三次元カメラは、制御装置を含み、
   前記制御装置は、
   前記三次元カメラから前記線状物の三次元形状を取得し、
   前記三次元形状から前記線状物の先端の位置を取得し、
   前記線状物の前記先端の位置に基づき、前記先端を目標位置に移動させるための情報を前記ロボットハンドを有する前記ロボットに通知する、三次元カメラ。

Images