知財求人 - 知財ポータルサイト「IP Force」
特開2024-97804線状物の挿入方法、制御装置、三次元カメラ、線状物の孔挿入システム
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024097804
(43)【公開日】2024-07-19
(54)【発明の名称】線状物の挿入方法、制御装置、三次元カメラ、線状物の孔挿入システム
(51)【国際特許分類】
B25J 13/08 20060101AFI20240711BHJP
【ＦＩ】
B25J13/08 A
【審査請求】有
【請求項の数】12
【出願形態】ＯＬ
(21)【出願番号】P 2024069252
(22)【出願日】2024-04-22
(62)【分割の表示】P 2019146818の分割
【原出願日】2019-08-08
(71)【出願人】
【識別番号】000001096
【氏名又は名称】倉敷紡績株式会社
(74)【代理人】
【識別番号】100167988
【弁理士】
【氏名又は名称】河原 哲郎
(72)【発明者】
【氏名】中島 大介
(72)【発明者】
【氏名】小寺 熱気
(57)【要約】
【課題】湾曲した線状物をロボットハンドで把持して孔に挿入する線状物の挿入方法を提供する。
【解決手段】ロボットハンド１２でチューブＴを把持する工程と、チューブＴの先端を孔Ｈに挿入する工程と、チューブＴの先端の挿入後、チューブＴの把持された部分の向きが孔Ｈの中心軸Ｃと実質的に一致するように、かつ、チューブＴの把持された部分が孔Ｈの中心Ｃ０から所定の距離ＺとなるようにチューブＴを孔Ｈに押し込む工程とを有する線状物の挿入方法。
【選択図】図１
【解決手段】ロボットハンド１２でチューブＴを把持する工程と、チューブＴの先端を孔Ｈに挿入する工程と、チューブＴの先端の挿入後、チューブＴの把持された部分の向きが孔Ｈの中心軸Ｃと実質的に一致するように、かつ、チューブＴの把持された部分が孔Ｈの中心Ｃ０から所定の距離ＺとなるようにチューブＴを孔Ｈに押し込む工程とを有する線状物の挿入方法。
【選択図】図１
【特許請求の範囲】
【請求項1】
湾曲した線状物をロボットハンドで把持して孔に挿入する線状物の挿入方法であって、
前記ロボットハンドの把持部が前記線状物を把持する工程と、
前記線状物の先端を前記孔に挿入する工程と、
前記先端の挿入後に、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込む工程とを有する、
線状物の挿入方法。
前記ロボットハンドの把持部が前記線状物を把持する工程と、
前記線状物の先端を前記孔に挿入する工程と、
前記先端の挿入後に、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込む工程とを有する、
線状物の挿入方法。
【請求項2】
前記把持する工程の後に、前記線状物の先端の位置及び向き情報を取得する工程をさらに有する、
請求項１に記載の線状物の挿入方法。
請求項１に記載の線状物の挿入方法。
【請求項3】
前記線状物の先端を前記孔に挿入する工程は、前記線状物の先端の向きが前記孔の中心軸と実質的に一致するように挿入する工程である、
請求項１または２に記載の線状物の挿入方法。
請求項１または２に記載の線状物の挿入方法。
【請求項4】
前記ロボットハンドは、前記ロボットハンドの把持部と前記線状物の把持された部分とが略垂直となるように前記線状物を把持する、
請求項１から３のいずれか一項に記載の線状物の挿入方法。
請求項１から３のいずれか一項に記載の線状物の挿入方法。
【請求項5】
前記線状物が透明であり、
前記ロボットハンドが前記線状物を把持する工程は、
背景を濃色にして順光で撮像した前記線状物の画像から前記線状物の三次元形状を取得する工程を含み、
前記線状物の三次元形状に基づいて前記ロボットハンドが前記線状物を把持する、
請求項１から４のいずれか一項に記載の線状物の挿入方法。
前記ロボットハンドが前記線状物を把持する工程は、
背景を濃色にして順光で撮像した前記線状物の画像から前記線状物の三次元形状を取得する工程を含み、
前記線状物の三次元形状に基づいて前記ロボットハンドが前記線状物を把持する、
請求項１から４のいずれか一項に記載の線状物の挿入方法。
【請求項6】
前記線状物が透明であり、
前記ロボットハンドが前記線状物を把持する工程は、
逆光で撮像した前記線状物の画像から前記線状物の三次元形状を取得する工程を含み、
前記線状物の三次元形状に基づいて前記ロボットハンドが前記線状物を把持する、
請求項１から４のいずれか一項に記載の線状物の挿入方法。
前記ロボットハンドが前記線状物を把持する工程は、
逆光で撮像した前記線状物の画像から前記線状物の三次元形状を取得する工程を含み、
前記線状物の三次元形状に基づいて前記ロボットハンドが前記線状物を把持する、
請求項１から４のいずれか一項に記載の線状物の挿入方法。
【請求項7】
前記線状物が可撓性を有する、
請求項１から６のいずれか一項に記載の線状物の挿入方法。
請求項１から６のいずれか一項に記載の線状物の挿入方法。
【請求項8】
前記線状物が、樹脂チューブである、
請求項７に記載の線状物の挿入方法。
請求項７に記載の線状物の挿入方法。
【請求項9】
湾曲した線状物をロボットハンドで把持して孔に挿入するために用いられるロボットを制御する制御装置であって、
前記線状物を把持するための情報と、
前記線状物の先端を孔に挿入させるための情報と、
前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記線状物の把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込むための情報と、を前記ロボットに通知する、
制御装置。
前記線状物を把持するための情報と、
前記線状物の先端を孔に挿入させるための情報と、
前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記線状物の把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込むための情報と、を前記ロボットに通知する、
制御装置。
【請求項10】
湾曲した線状物をロボットハンドで把持して孔に挿入するロボットの動作を制御するために用いられる三次元カメラであって、
当該三次元カメラは、カメラ制御装置を含み、
前記カメラ制御装置は、
前記三次元カメラで撮像した画像から前記線状物の三次元形状を取得し、
前記三次元形状に基づいて、前記線状物の先端を孔に挿入するための情報を前記ロボットに通知し、
前記三次元形状に基づいて、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記線状物の把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込むための情報を前記ロボットに通知する、
三次元カメラ。
当該三次元カメラは、カメラ制御装置を含み、
前記カメラ制御装置は、
前記三次元カメラで撮像した画像から前記線状物の三次元形状を取得し、
前記三次元形状に基づいて、前記線状物の先端を孔に挿入するための情報を前記ロボットに通知し、
前記三次元形状に基づいて、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記線状物の把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込むための情報を前記ロボットに通知する、
三次元カメラ。
【請求項11】
請求項１０に記載の三次元カメラと、
前記線状物を把持するロボットハンドを備えたロボットとを有する、
線状物の孔挿入システム。
前記線状物を把持するロボットハンドを備えたロボットとを有する、
線状物の孔挿入システム。
【請求項12】
前記線状物を把持するロボットハンドを備えたロボットと、三次元計測装置と、請求項９に記載の制御装置とを有する、
線状物の孔挿入システム。
線状物の孔挿入システム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、湾曲している線状物をロボットハンドで把持して孔へ挿入する線状物の挿入方法、その挿入方法に用いられる制御装置、三次元カメラ、線状物の孔挿入システムに関する。
【背景技術】
【0002】
近年、様々な作業がロボットハンドを用いて行われている。そのようなロボットハンドによる作業において、線状物を操作する技術が開示されている。例えば、特許文献１には、ケーブルの先端をロボット装置に把持させ、コネクタに接続する技術が開示されている。
本出願人は、特許文献２に示すように、複数の線状物からロボットハンドに把持させる注目線状物を決定する方法を提案している。
本出願人は、特許文献２に示すように、複数の線状物からロボットハンドに把持させる注目線状物を決定する方法を提案している。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０１４－１７６９１７号公報
【特許文献2】ＷＯ２０１９／０９８０７４
【発明の概要】
【発明が解決しようとする課題】
【0004】
また本出願人は、特願２０１８－３１２６３に示すように、複数の線状物から注目線状物を把持させ、その注目線状物の先端を孔（目標位置）まで移動させる方法を提案している。しかし、線状物が湾曲している場合、線状物の先端を孔（目標位置）に移動しただけでは、線状物を孔にスムースに挿入することができないという新たな問題が見つかった。詳しくは、目標位置におけるロボットハンドの姿勢を維持したまま線状物を孔に押し込む場合、ロボットハンドが孔の開口縁と衝突し、線状物をロボットハンドの把持している位置まで深く押し込むことができない。また、ロボットハンドが押し込み動作をしている最中に、湾曲した線状物が孔の中で引っかかり、線状物を孔の奥深くまで押し込むことができない場合がある。
このように本発明は、線状物をロボットハンドで把持して線状物の先端を所定の位置まで移動させた後、線状物の一部を孔へ挿入する線状物の挿入方法、その挿入方法に用いられる制御装置、三次元カメラ、線状物の孔挿入システムを提供することを目的としている。
このように本発明は、線状物をロボットハンドで把持して線状物の先端を所定の位置まで移動させた後、線状物の一部を孔へ挿入する線状物の挿入方法、その挿入方法に用いられる制御装置、三次元カメラ、線状物の孔挿入システムを提供することを目的としている。
【課題を解決するための手段】
【0005】
本発明の線状物の挿入方法は、湾曲した線状物をロボットハンドで把持して孔に挿入する線状物の挿入方法であって、前記ロボットハンドの把持部が前記線状物を把持する工程と、前記線状物の先端を前記孔に挿入する工程と、前記先端の挿入後に、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込む工程とを有することを特徴としている。
本発明の線状物の挿入方法は、線状物の先端を挿入した後、線状物の把持された部分の向きが孔の中心軸と実質的に一致するように、かつ、把持された部分が孔の中心から所定の距離となるようにロボットハンドを移動させて線状物を孔に押し込むため、線状物をロボットハンドが把持している部分近辺まで確実に挿入することができる。
本発明の線状物の挿入方法は、線状物の先端を挿入した後、線状物の把持された部分の向きが孔の中心軸と実質的に一致するように、かつ、把持された部分が孔の中心から所定の距離となるようにロボットハンドを移動させて線状物を孔に押し込むため、線状物をロボットハンドが把持している部分近辺まで確実に挿入することができる。
【0006】
本発明の線状物の挿入方法であって、前記把持する工程の後に、前記線状物の先端の位置及び向き情報を取得する工程をさらに有するのが好ましい。線状物が可撓性を有している場合、ロボットハンドの把持によって線状物の湾曲形状が変わることがある。把持後の線状物の先端の位置及び向き情報を取得することにより、線状物の湾曲形状が変わっても確実に線状物を孔に挿入することができる。
本発明の線状物の挿入方法であって、前記線状物の先端を前記孔に挿入する工程が、前記線状物の先端の向きが前記孔の中心軸と実質的に一致するように挿入する工程であるのが好ましい。この場合、線状物の先端を確実に孔に挿入することができる。
本発明の線状物の挿入方法であって、前記ロボットハンドは、前記ロボットハンドの把持部と前記線状物の把持された部分とが略垂直となるように前記線状物を把持するのが好ましい。つまり、ロボットハンドの把持部が前記孔の中心軸と略垂直となるようにして前記線状物を前記孔に押し込むのが好ましい。
本発明の線状物の挿入方法であって、前記線状物の先端を前記孔に挿入する工程が、前記線状物の先端の向きが前記孔の中心軸と実質的に一致するように挿入する工程であるのが好ましい。この場合、線状物の先端を確実に孔に挿入することができる。
本発明の線状物の挿入方法であって、前記ロボットハンドは、前記ロボットハンドの把持部と前記線状物の把持された部分とが略垂直となるように前記線状物を把持するのが好ましい。つまり、ロボットハンドの把持部が前記孔の中心軸と略垂直となるようにして前記線状物を前記孔に押し込むのが好ましい。
【0007】
本発明の線状物の挿入方法であって、前記線状物が透明であり、前記ロボットハンドが前記線状物を把持する工程が、背景を濃色にして順光で撮像した前記線状物の画像から前記線状物の三次元形状を取得する工程を含み、または、逆光で撮像した前記線状物の画像から前記線状物の三次元形状を取得する工程を含み、その線状物の三次元形状に基づいて前記ロボットハンドが前記線状物を把持するのが好ましい。この場合、カメラ等によって線状物の形状を明確に撮像することができ、詳細な線状物の三次元形状を取得することができる。
【0008】
本発明の線状物の挿入方法は、可撓性を有する線状物に好ましい。特に、樹脂チューブが好ましい。
【0009】
本発明の制御装置は、湾曲した線状物をロボットハンドで把持して孔に挿入するために用いられるロボットを制御する制御装置であって、前記線状物を把持するための情報と、前記線状物の先端を孔に挿入させるための情報と、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記線状物の把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込むための情報と、を前記ロボットに通知することを特徴としている。なお、線状物を前記孔に押し込むための情報の通知は、前記線状物の先端を孔に挿入する前であっても、挿入する後であってもよい。
【0010】
本発明の三次元カメラは、湾曲した線状物をロボットハンドで把持して孔に挿入するロボットの動作を制御するために用いられる三次元カメラであって、当該三次元カメラは、カメラ制御装置を含み、前記カメラ制御装置は、前記三次元カメラで撮像した画像から前記線状物の三次元形状を取得し、前記三次元形状に基づいて、前記線状物の先端を孔に挿入するための情報を前記ロボットに通知し、前記三次元形状に基づいて、前記線状物の把持された部分の向きが前記孔の中心軸と実質的に一致するように、かつ、前記線状物の把持された部分が前記孔の中心から所定の距離となるように前記線状物を前記孔に押し込むための情報を前記ロボットに通知することを特徴としている。
【0011】
本発明の線状物の孔挿入システムは、本発明の三次元カメラと、前記線状物を把持するロボットハンドを備えたロボットとを有することを特徴としている。
本発明の線状物の孔挿入システムの第２の態様は、線状物を把持するロボットハンドを備えたロボットと、三次元計測装置と、本発明の制御装置とを有することを特徴としている。
本発明の線状物の孔挿入システムの第２の態様は、線状物を把持するロボットハンドを備えたロボットと、三次元計測装置と、本発明の制御装置とを有することを特徴としている。
【発明の効果】
【0012】
本発明によれば、湾曲した線状物であっても、確実に線状物を孔へ挿入し、孔の奥まで押し込むことができる。これにより煩雑な湾曲した線状物の挿入作業が簡素化できる。
【図面の簡単な説明】
【0013】
【図1】本発明の線状物の挿入方法を実施するための線状物の孔挿入システムを示す概略図である。
【図2】本発明の線状物の挿入方法の一実施形態を示すフロー図である。
【発明を実施するための形態】
【0014】
図１にチューブＴ（線状物）の挿入方法を実施するための線状物の孔挿入システム１を示す。線状物の孔挿入システム１は、ロボットハンド１２を有するロボット１０と、三次元計測装置（三次元カメラ）２０と、制御装置３０とを有する。作業空間には、チューブＴが収容された収納筒Ｓと、チューブＴを挿入する孔Ｈとが配置されている。収納筒Ｓには、チューブＴの先端が突出して挿入されている。
この線状物の孔挿入システム１は、収納筒Ｓに収容された湾曲したチューブＴをロボットハンド１２で把持し、収納筒Ｓから取り出し、チューブＴを孔Ｈに挿入する。
この線状物の孔挿入システム１は、収納筒Ｓに収容された湾曲したチューブＴをロボットハンド１２で把持し、収納筒Ｓから取り出し、チューブＴを孔Ｈに挿入する。
【0015】
初めにチューブＴについて説明する。
チューブＴは、可撓性を有する透明な樹脂チューブである。ここで可撓性とは、チューブＴをロボットハンド等で把持したとき、把持した位置から先端までが自重により弾力的に撓むものをいう。つまり、自重により撓んでいるチューブＴの軸線は、鉛直面上において、一方向に湾曲した連続曲線として表すことができる。
ここで、透明とは半透明も含み、透明なチューブの材料としては、ナイロン、ポリアミド、アクリル、ポリプロピレン、ポリ塩化ビニルなどの透明な樹脂、ガラス、ポリアミドエラストマー、シリコン、テフロン（登録商標）が好ましく挙げられる。しかし、非透明なチューブであってもよい。そのような非透明チューブとしては工業用チューブなどが挙げられる。またチューブ以外の可撓性を有する線状物であってもよい。チューブ以外の可撓性を有する線状物としては、例えば、電線、ワイヤーハーネス、紐、糸、繊維、ガラス繊維、光ファイバ、乾麺等が挙げられる。
チューブＴの径としては、例えば、０．１ｍｍ～３ｍｍ、好ましくは０．１５ｍｍ～２ｍｍ、特にこのましくは０．２ｍｍ～１ｍｍである。
チューブＴは、可撓性を有する透明な樹脂チューブである。ここで可撓性とは、チューブＴをロボットハンド等で把持したとき、把持した位置から先端までが自重により弾力的に撓むものをいう。つまり、自重により撓んでいるチューブＴの軸線は、鉛直面上において、一方向に湾曲した連続曲線として表すことができる。
ここで、透明とは半透明も含み、透明なチューブの材料としては、ナイロン、ポリアミド、アクリル、ポリプロピレン、ポリ塩化ビニルなどの透明な樹脂、ガラス、ポリアミドエラストマー、シリコン、テフロン（登録商標）が好ましく挙げられる。しかし、非透明なチューブであってもよい。そのような非透明チューブとしては工業用チューブなどが挙げられる。またチューブ以外の可撓性を有する線状物であってもよい。チューブ以外の可撓性を有する線状物としては、例えば、電線、ワイヤーハーネス、紐、糸、繊維、ガラス繊維、光ファイバ、乾麺等が挙げられる。
チューブＴの径としては、例えば、０．１ｍｍ～３ｍｍ、好ましくは０．１５ｍｍ～２ｍｍ、特にこのましくは０．２ｍｍ～１ｍｍである。
【0016】
収納筒Ｓは、チューブＴを挿入して収容するものである。チューブＴを挿入したとき、チューブＴの一部が突出する長さになっている。収納筒Ｓは、作業空間内において固定されている。なお、チューブＴは必ずしも収納筒に収納されている必要はなく、作業台等に置かれていてもよい。収納筒からチューブＴの把持される部分が突出しているなど、把持される部分がどこにも接地していない方がロボットハンドで把持しやすく好適である。
孔Ｈは、円形となっており、その内面は円筒となっている。孔Ｈは、作業空間内において固定されており、水平方向に開口している。なお、孔Ｈはテーパー構造であってもよい。孔Ｈが開口面から奥に向かって径が小さくなるテーパー構造である場合、本発明の効果がより顕著に得られる。チューブに対する孔のクリアランスが小さい場合、孔の前にテーパー構造の挿入補助治具を設けて、実質的に孔の開口部の径を広げ、本発明を適用することも可能である。孔Ｈの向きは特に限定されない。孔Ｈの径は、挿入するチューブの径プラス０．５ｍｍ以上、１０ｍｍ以下が好ましい。孔Ｈの直径がチューブの径プラス１０ｍｍより大きい場合、押し込み動作中にチューブが孔の途中で引っかかり、挿入が阻害されることが少なく、本発明を適用する必要性が少ないためである。
なお、孔Ｈは、円でなくてもよい。その場合、孔Ｈの内接円の径が、この明細書でいう孔の径となる。
孔Ｈは、円形となっており、その内面は円筒となっている。孔Ｈは、作業空間内において固定されており、水平方向に開口している。なお、孔Ｈはテーパー構造であってもよい。孔Ｈが開口面から奥に向かって径が小さくなるテーパー構造である場合、本発明の効果がより顕著に得られる。チューブに対する孔のクリアランスが小さい場合、孔の前にテーパー構造の挿入補助治具を設けて、実質的に孔の開口部の径を広げ、本発明を適用することも可能である。孔Ｈの向きは特に限定されない。孔Ｈの径は、挿入するチューブの径プラス０．５ｍｍ以上、１０ｍｍ以下が好ましい。孔Ｈの直径がチューブの径プラス１０ｍｍより大きい場合、押し込み動作中にチューブが孔の途中で引っかかり、挿入が阻害されることが少なく、本発明を適用する必要性が少ないためである。
なお、孔Ｈは、円でなくてもよい。その場合、孔Ｈの内接円の径が、この明細書でいう孔の径となる。
【0017】
次に線状物の孔挿入システム１について説明する。
ロボット１０は、多関節アーム１１と、その先端に設けられたロボットハンド１２とを有し、ロボットハンド１２が一対のフィンガ（把持部）１２ａを備えた公知のものである。フィンガ１２ａとしては、特に限定されるものではなく、チューブ（線状物）Ｔを面状または線状で接触して把持するものや、点で把持するものが挙げられる。
ロボット１０は、多関節アーム１１と、その先端に設けられたロボットハンド１２とを有し、ロボットハンド１２が一対のフィンガ（把持部）１２ａを備えた公知のものである。フィンガ１２ａとしては、特に限定されるものではなく、チューブ（線状物）Ｔを面状または線状で接触して把持するものや、点で把持するものが挙げられる。
【0018】
三次元計測装置２０は、第１カメラ２１及び第２カメラ２２と、カメラ制御部２３とを有するステレオカメラである。第１カメラ２１は、カラーの二次元画像である第１画像を撮像するカラーカメラであり、第２カメラ２２は、カラーの二次元画像である第２画像を撮像するカラーカメラである。第１カメラ２１および第２カメラ２２は、異なる位置に固定されている。カメラ制御部２３は、第１カメラ２１および第２カメラ２２を制御し、チューブＴの三次元形状を算出し、制御装置３０と通信を行う。詳しくは、制御装置３０から撮像指示を受信して第１カメラ２１および第２カメラ２２に撮像を指示し、第１画像および第２画像からチューブＴの三次元形状を算出し、その三次元形状を制御装置３０に送信する。なお、第１画像および第２画像は、作業空間におけるチューブＴの撮像位置において、背景を濃色として順光でチューブＴの反射光を撮像するか、または、透過照明を用いて逆光でチューブＴを撮像し、チューブＴのシルエット画像を取得して三次元形状を算出するのが好ましい。このように撮像することで、チューブＴが透明であっても、撮像して得られる画像において、チューブＴと背景との輝度の差が大きくなるため、チューブＴの形状を明確に露出させることができ、より精確にチューブＴの三次元形状を取得することができるためである。
なお、順光とは、カメラ側からチューブＴへ光が照射されている状態を意味し、逆光とは、チューブＴの背後側から光が照射されている状態を意味する。
ステレオカメラによる三次元形状の算出方法としては、第１カメラの第１画像及び第２カメラの第２画像から計測したい計測点を求め、第１カメラと第２カメラの位置関係から三角測量の原理によって計測点の三次元座標を算出する。
なお、三次元計測装置２０は、ステレオカメラに限定されるものではなく、線状物の三次元形状を取得できるものであれば、三次元スキャナーなどを用いてもよい。また、線状物全体の三次元形状を算出する必要はなく、少なくとも線状物上のロボットハンドに把持される部分の位置および向きと、孔に挿入される側の先端の位置および向きを算出できればよい。
なお、順光とは、カメラ側からチューブＴへ光が照射されている状態を意味し、逆光とは、チューブＴの背後側から光が照射されている状態を意味する。
ステレオカメラによる三次元形状の算出方法としては、第１カメラの第１画像及び第２カメラの第２画像から計測したい計測点を求め、第１カメラと第２カメラの位置関係から三角測量の原理によって計測点の三次元座標を算出する。
なお、三次元計測装置２０は、ステレオカメラに限定されるものではなく、線状物の三次元形状を取得できるものであれば、三次元スキャナーなどを用いてもよい。また、線状物全体の三次元形状を算出する必要はなく、少なくとも線状物上のロボットハンドに把持される部分の位置および向きと、孔に挿入される側の先端の位置および向きを算出できればよい。
【0019】
制御装置３０は、三次元計測装置２０と通信し、三次元計測装置２０から取得したチューブＴの三次元形状に基づいてロボット１０を指示し、そして、各種演算を行う。なお、制御装置３０は、ロボット１０および三次元計測装置２０と独立した制御装置としても、三次元計測装置２０が備えるカメラ制御部２３、ロボット１０が備えるロボット制御装置のいずれであってもよい。制御装置３０には、収納筒Ｓおよび孔Ｈの位置が予め登録されている。
【0020】
次に、図２に基づいて、収納筒ＳのチューブＴをロボットハンド１２で取り出して孔Ｈに挿入する線状物の挿入方法について説明する。
線状物の挿入方法は、チューブＴを撮像してチューブＴの三次元形状を取得する工程１と、チューブＴの把持される部分Ｇの位置を算出する工程２と、ロボットハンド１２の把持姿勢および把持位置を算出する工程３と、ロボットハンドでチューブＴを把持する工程４と、チューブＴを第２撮像位置まで移動する工程５と、第２撮像位置で把持された状態のチューブＴを撮像して、チューブＴの三次元形状を取得する工程６と、把持された状態のチューブＴ（挿入部Ｉ）の先端Ｅの位置および向きを算出する工程７と、ロボットハンドを挿入手前位置まで移動させる工程８と、ロボットハンドを挿入位置まで移動させる工程９と、チューブＴを孔Ｈに押し込んでロボットハンドを挿入完了位置まで移動させる工程１０とを有する。
線状物の挿入方法は、チューブＴを撮像してチューブＴの三次元形状を取得する工程１と、チューブＴの把持される部分Ｇの位置を算出する工程２と、ロボットハンド１２の把持姿勢および把持位置を算出する工程３と、ロボットハンドでチューブＴを把持する工程４と、チューブＴを第２撮像位置まで移動する工程５と、第２撮像位置で把持された状態のチューブＴを撮像して、チューブＴの三次元形状を取得する工程６と、把持された状態のチューブＴ（挿入部Ｉ）の先端Ｅの位置および向きを算出する工程７と、ロボットハンドを挿入手前位置まで移動させる工程８と、ロボットハンドを挿入位置まで移動させる工程９と、チューブＴを孔Ｈに押し込んでロボットハンドを挿入完了位置まで移動させる工程１０とを有する。
【0021】
チューブＴを撮像してチューブＴの三次元形状を取得する（工程１）。
工程１は、上述の三次元計測装置２０によって、収納筒Ｓから突出したチューブＴを第１撮像位置で計測し、その三次元形状を算出する。詳しくは、ステレオカメラによって、チューブＴが挿入された収納筒Ｓのある作業空間を撮像し、２枚の画像を演算処理して収納筒Ｓから突出したチューブＴの三次元形状を取得する。チューブＴの三次元形状は、直交座標系または斜交座標系で表され、好ましくは直交座標系で表される。
工程１は、上述の三次元計測装置２０によって、収納筒Ｓから突出したチューブＴを第１撮像位置で計測し、その三次元形状を算出する。詳しくは、ステレオカメラによって、チューブＴが挿入された収納筒Ｓのある作業空間を撮像し、２枚の画像を演算処理して収納筒Ｓから突出したチューブＴの三次元形状を取得する。チューブＴの三次元形状は、直交座標系または斜交座標系で表され、好ましくは直交座標系で表される。
【0022】
チューブＴのロボットハンドに把持される部分Ｇの位置を算出する（工程２）。
工程２は、図３ａに示すように、チューブＴの先端から所定の長さＸをロボットハンドに把持される部分Ｇとするなど、予め定めた条件に基づいて、チューブＴの三次元形状からチューブＴの把持される部分Ｇを決定し、その把持される部分Ｇの位置（座標）を取得する。
先端Ｅから把持される部分Ｇまでの挿入部Ｉの長さＸは、１０ｍｍ～１００ｍｍ、好ましくは１５ｍｍ～９０ｍｍ、特に好ましくは２０ｍｍ～８０ｍｍである。この挿入方法では、このチューブＴの挿入部Ｉが挿入される。長さＸが１００ｍｍより長いと、挿入部Ｉの撓みが大きくなりすぎて、工程１０の押し込み工程ができなくなるおそれが生じる。
工程２は、図３ａに示すように、チューブＴの先端から所定の長さＸをロボットハンドに把持される部分Ｇとするなど、予め定めた条件に基づいて、チューブＴの三次元形状からチューブＴの把持される部分Ｇを決定し、その把持される部分Ｇの位置（座標）を取得する。
先端Ｅから把持される部分Ｇまでの挿入部Ｉの長さＸは、１０ｍｍ～１００ｍｍ、好ましくは１５ｍｍ～９０ｍｍ、特に好ましくは２０ｍｍ～８０ｍｍである。この挿入方法では、このチューブＴの挿入部Ｉが挿入される。長さＸが１００ｍｍより長いと、挿入部Ｉの撓みが大きくなりすぎて、工程１０の押し込み工程ができなくなるおそれが生じる。
【0023】
ロボットハンド１２の把持姿勢および把持位置を算出する（工程３）。
ロボットハンドの把持姿勢は、図３ａ、図３ｂに示すように、ロボットハンド１２でチューブＴの把持される部分Ｇを把持させたとき、ロボットハンド１２のフィンガ１２ａの向きＦ１とチューブＴの把持される部分Ｇの向きＤ１とが略垂直となる姿勢が好ましい。後述する線状物の押し込み工程で、フィンガ１２ａと孔Ｈとが干渉しにくく、フィンガの向きＦ１と孔の中心軸とを略垂直にすることで、線状物を孔のより深くまで押し込むことが可能なためである。
ロボットハンドの把持方向は、チューブＴの把持される部分Ｇの軸回りに３６０度のいずれの方向から把持してもよい。しかし、ロボットハンド１２は湾曲しているチューブＴの凸側に位置するのが好ましい。特に、湾曲しているチューブＴの軸線Ｔ０を含む鉛直面Ｓｐと平行となるようにしてチューブＴの凸側に位置するのが好ましい。ここでロボットハンド１２が、鉛直面Ｓｐと平行になるとは、一対のフィンガ１２ａの当接面が鉛直面Ｓｐと平行であり、かつ、一対のフィンガ１２ａの中心がその鉛直面上にあることをいう。そして、ロボットハンド１２を湾曲しているチューブＴの凸側からチューブＴに近づかせ、一対のフィンガ１２ａでチューブＴの把持される部分Ｇを把持させたときの姿勢が好ましい。
なお、ロボットハンド１２のフィンガ１２ａの向きＦ１は、フィンガ１２ａの先端の向きまたは一対のフィンガ１２ａを含む平面の向き（図３ａの符号Ｆ１を含む紙面方向の面）をいい、特に、フィンガ１２ａがチューブＴを把持する際にチューブＴに向かってアプローチする向きをいう。
ロボットハンドに把持される部分Ｇの向きＤ１は、チューブＴの三次元形状よりフィンガ１２ａで把持される部分の線分Ｌ１の平均ベクトルを求めることにより算出する。線分Ｌ１の長さは、例えばチューブＴの把持される位置を中心としたフィンガ１２ａの厚み分の長さとするなど、予め定めた所定の長さに設定することができる。
ロボットハンドの把持姿勢は、図３ａ、図３ｂに示すように、ロボットハンド１２でチューブＴの把持される部分Ｇを把持させたとき、ロボットハンド１２のフィンガ１２ａの向きＦ１とチューブＴの把持される部分Ｇの向きＤ１とが略垂直となる姿勢が好ましい。後述する線状物の押し込み工程で、フィンガ１２ａと孔Ｈとが干渉しにくく、フィンガの向きＦ１と孔の中心軸とを略垂直にすることで、線状物を孔のより深くまで押し込むことが可能なためである。
ロボットハンドの把持方向は、チューブＴの把持される部分Ｇの軸回りに３６０度のいずれの方向から把持してもよい。しかし、ロボットハンド１２は湾曲しているチューブＴの凸側に位置するのが好ましい。特に、湾曲しているチューブＴの軸線Ｔ０を含む鉛直面Ｓｐと平行となるようにしてチューブＴの凸側に位置するのが好ましい。ここでロボットハンド１２が、鉛直面Ｓｐと平行になるとは、一対のフィンガ１２ａの当接面が鉛直面Ｓｐと平行であり、かつ、一対のフィンガ１２ａの中心がその鉛直面上にあることをいう。そして、ロボットハンド１２を湾曲しているチューブＴの凸側からチューブＴに近づかせ、一対のフィンガ１２ａでチューブＴの把持される部分Ｇを把持させたときの姿勢が好ましい。
なお、ロボットハンド１２のフィンガ１２ａの向きＦ１は、フィンガ１２ａの先端の向きまたは一対のフィンガ１２ａを含む平面の向き（図３ａの符号Ｆ１を含む紙面方向の面）をいい、特に、フィンガ１２ａがチューブＴを把持する際にチューブＴに向かってアプローチする向きをいう。
ロボットハンドに把持される部分Ｇの向きＤ１は、チューブＴの三次元形状よりフィンガ１２ａで把持される部分の線分Ｌ１の平均ベクトルを求めることにより算出する。線分Ｌ１の長さは、例えばチューブＴの把持される位置を中心としたフィンガ１２ａの厚み分の長さとするなど、予め定めた所定の長さに設定することができる。
【0024】
このようにロボットハンド１２の把持位置および把持姿勢とは、図３ｂに示すように、ロボットハンド１２が把持される部分Ｇを把持したときのロボットハンド１２の座標及び姿勢である。
そして、フィンガ１２ａの向きＦ１と把持される部分Ｇの向きＤ１とを略垂直として、ロボットハンド１２にチューブＴを把持させることにより、後述するように、工程１０において、孔へチューブＴを挿入部Ｉの奥（把持される部分Ｇ直前）までしっかり挿入することができる。またロボットハンド１２を、上記鉛直面のチューブの凸側に位置させ、かつ、当該鉛直面と平行にすることにより、チューブＴの撓んだ先端部分等に干渉することなく、ロボットハンドでチューブＴの把持位置を把持することが可能である。
そして、フィンガ１２ａの向きＦ１と把持される部分Ｇの向きＤ１とを略垂直として、ロボットハンド１２にチューブＴを把持させることにより、後述するように、工程１０において、孔へチューブＴを挿入部Ｉの奥（把持される部分Ｇ直前）までしっかり挿入することができる。またロボットハンド１２を、上記鉛直面のチューブの凸側に位置させ、かつ、当該鉛直面と平行にすることにより、チューブＴの撓んだ先端部分等に干渉することなく、ロボットハンドでチューブＴの把持位置を把持することが可能である。
【0025】
ロボットハンド１２でチューブＴを把持する（工程４）。
工程４は、ロボットハンド１２を待機位置から把持位置まで移動させ、把持位置においてチューブＴの把持される部分Ｇを把持する。
ロボットハンド１２の待機位置は、特に限定されるものではなく、予め定めておいてもよく、例えば、チューブＴから把持方向と逆向きに所定距離離れた位置（例えば、図３ａのロボットハンド１２の位置）に決定するなどチューブＴの三次元形状に基づいて決定させてもよい。しかし、例えば、ロボットの各リンクの角度などによって、待機位置の座標および姿勢を予め定めて制御装置に登録しておいてもよい。同種のチューブを複数回把持する場合、チューブの撓み方等が似ているため、１本目の把持位置および把持姿勢を予め登録しておけば、その後、同種のチューブを同じ把持位置および把持姿勢で把持できることが多いためである。
ロボットハンド１２の待機位置から把持位置までの軌跡は、特に限定されるものではない。ロボットハンド１２が収納筒Ｓ等の障害物と接触しないように、ロボットハンド１２を把持される部分Ｇへ接近させる。その軌跡の算出は、例えば、待機位置から把持位置の軌跡が直線となるように軌跡を算出する直線補間、待機位置から把持位置の軌跡が円弧となるように軌跡を算出する円弧補間、待機位置及び把持位置における各リンクの関節角度差を均等に補間させて軌跡を算出する関節補間などの目的地に漸次向かわせる手段によってできる。
工程４は、ロボットハンド１２を待機位置から把持位置まで移動させ、把持位置においてチューブＴの把持される部分Ｇを把持する。
ロボットハンド１２の待機位置は、特に限定されるものではなく、予め定めておいてもよく、例えば、チューブＴから把持方向と逆向きに所定距離離れた位置（例えば、図３ａのロボットハンド１２の位置）に決定するなどチューブＴの三次元形状に基づいて決定させてもよい。しかし、例えば、ロボットの各リンクの角度などによって、待機位置の座標および姿勢を予め定めて制御装置に登録しておいてもよい。同種のチューブを複数回把持する場合、チューブの撓み方等が似ているため、１本目の把持位置および把持姿勢を予め登録しておけば、その後、同種のチューブを同じ把持位置および把持姿勢で把持できることが多いためである。
ロボットハンド１２の待機位置から把持位置までの軌跡は、特に限定されるものではない。ロボットハンド１２が収納筒Ｓ等の障害物と接触しないように、ロボットハンド１２を把持される部分Ｇへ接近させる。その軌跡の算出は、例えば、待機位置から把持位置の軌跡が直線となるように軌跡を算出する直線補間、待機位置から把持位置の軌跡が円弧となるように軌跡を算出する円弧補間、待機位置及び把持位置における各リンクの関節角度差を均等に補間させて軌跡を算出する関節補間などの目的地に漸次向かわせる手段によってできる。
【0026】
チューブＴを第２撮像位置まで移動する（工程５）。
ロボットハンドの第２撮像位置は、工程６において、ロボットハンド１２に把持された状態のチューブＴを撮像する位置である。この第２撮像位置の座標は、特に限定されないが、収納筒Ｓと孔Ｈの間の位置でもよく、チューブＴを完全に収納筒Ｓから完全に取り出した位置でもよく、チューブＴを収納筒Ｓから完全に取り出す前の位置でもよい。例えば、１台の三次元計測装置２０の撮像範囲内に、工程１の第１撮像位置と工程５の第２撮像位置を含めることにより、１つの三次元計測装置２０で把持される前のチューブＴと、把持された後のチューブＴとの両方の画像を取得することができて好ましい。
このようなロボットハンド１２の第２撮像位置（座標および姿勢）は、例えば、ロボットの各リンクの角度などによって、その座標および撮像姿勢を予め定めて制御装置に登録しておくのが好ましい。
ロボットハンドの第２撮像位置は、工程６において、ロボットハンド１２に把持された状態のチューブＴを撮像する位置である。この第２撮像位置の座標は、特に限定されないが、収納筒Ｓと孔Ｈの間の位置でもよく、チューブＴを完全に収納筒Ｓから完全に取り出した位置でもよく、チューブＴを収納筒Ｓから完全に取り出す前の位置でもよい。例えば、１台の三次元計測装置２０の撮像範囲内に、工程１の第１撮像位置と工程５の第２撮像位置を含めることにより、１つの三次元計測装置２０で把持される前のチューブＴと、把持された後のチューブＴとの両方の画像を取得することができて好ましい。
このようなロボットハンド１２の第２撮像位置（座標および姿勢）は、例えば、ロボットの各リンクの角度などによって、その座標および撮像姿勢を予め定めて制御装置に登録しておくのが好ましい。
【0027】
把持された状態のチューブＴの三次元形状を取得する（工程６）。
工程６は、第２撮像位置において工程１と同様に三次元計測装置２０によって把持された状態のチューブＴを撮像し、その画像から把持されたチューブＴの三次元形状を算出する。これにより挿入直前の把持された状態のチューブＴ（挿入部Ｉ）の詳細な三次元形状を取得することができる。つまり、上述したようにチューブＴは可撓性を有しているため、ロボットハンドで把持したときのチューブＴの三次元形状は、収納筒Ｓに収納されたときのチューブＴの三次元形状と異なる場合がある（例えば、図３ｂと図３ｃ）。したがって、第２撮像位置に置いて、把持された状態のチューブＴの三次元形状を再度算出する。しかし、チューブＴがある程度硬く、チューブＴの先端の位置が実質的に変化しない場合は、工程５～工程７は省略し、工程１のチューブＴの三次元形状を把持された状態のチューブＴの挿入部Ｉの三次元形状として取り扱ってもよい。
なお、挿入部Ｉの三次元形状も、直交座標系で表すのが好ましい。
工程６は、第２撮像位置において工程１と同様に三次元計測装置２０によって把持された状態のチューブＴを撮像し、その画像から把持されたチューブＴの三次元形状を算出する。これにより挿入直前の把持された状態のチューブＴ（挿入部Ｉ）の詳細な三次元形状を取得することができる。つまり、上述したようにチューブＴは可撓性を有しているため、ロボットハンドで把持したときのチューブＴの三次元形状は、収納筒Ｓに収納されたときのチューブＴの三次元形状と異なる場合がある（例えば、図３ｂと図３ｃ）。したがって、第２撮像位置に置いて、把持された状態のチューブＴの三次元形状を再度算出する。しかし、チューブＴがある程度硬く、チューブＴの先端の位置が実質的に変化しない場合は、工程５～工程７は省略し、工程１のチューブＴの三次元形状を把持された状態のチューブＴの挿入部Ｉの三次元形状として取り扱ってもよい。
なお、挿入部Ｉの三次元形状も、直交座標系で表すのが好ましい。
【0028】
把持された状態のチューブＴ（挿入部Ｉ）の先端Ｅの位置および向きを算出する（工程７）。
先端Ｅの位置は、挿入部Ｉの三次元形状に基づいて先端Ｅの座標を求める。また、例えば、チューブＴは一方向に湾曲した状態となっているため、把持された部分Ｇからどれだけの距離にあるかを計測してもよい。
チューブＴの先端Ｅの向きＤ２は、挿入部Ｉの三次元形状より、先端Ｅから所定の長さ（例えば、約１０ｍｍ）の線分Ｌ２の平均ベクトルを求めることにより算出する。
先端Ｅの位置は、挿入部Ｉの三次元形状に基づいて先端Ｅの座標を求める。また、例えば、チューブＴは一方向に湾曲した状態となっているため、把持された部分Ｇからどれだけの距離にあるかを計測してもよい。
チューブＴの先端Ｅの向きＤ２は、挿入部Ｉの三次元形状より、先端Ｅから所定の長さ（例えば、約１０ｍｍ）の線分Ｌ２の平均ベクトルを求めることにより算出する。
【0029】
ロボットハンドを挿入手前位置まで移動させる（工程８）。
工程８は、ロボットハンドの挿入手前位置を算出し、ロボットハンド１２を第２撮像位置から挿入手前位置まで移動させる。
ロボットハンドの挿入手前位置は、孔の近傍の任意の場所に設定することができるが、好ましくは図４ａに示すように、ロボットハンドがチューブＴを把持した状態で、チューブＴの先端Ｅの向きＤ２が略水平（孔Ｈの中心軸Ｃと実質的に一致）となり、かつ、そのチューブＴの先端Ｅが孔Ｈの中心Ｃ０に対して所定の距離Ｙとなったときのロボットハンドの位置（座標および姿勢）である。このような状態となるときのロボットの各リンクの角度を算出する。
なお、ロボットハンド１２が挿入手前位置にあるときのチューブＴの先端Ｅの座標は、孔Ｈ近辺となるのが好ましい。例えば、チューブＴの先端Ｅが孔Ｈの中心Ｃ０に対する距離Ｙは、５～１００ｍｍ、好ましくは７ｍｍ～７０ｍｍ、特に好ましくは１０～５０ｍｍとなる位置が挙げられる。
第２撮像位置から挿入手前位置までの軌跡は、特に限定されるものではなく、好ましくは直線補間、円弧補間、関節補間等で算出する。
工程８は、ロボットハンドの挿入手前位置を算出し、ロボットハンド１２を第２撮像位置から挿入手前位置まで移動させる。
ロボットハンドの挿入手前位置は、孔の近傍の任意の場所に設定することができるが、好ましくは図４ａに示すように、ロボットハンドがチューブＴを把持した状態で、チューブＴの先端Ｅの向きＤ２が略水平（孔Ｈの中心軸Ｃと実質的に一致）となり、かつ、そのチューブＴの先端Ｅが孔Ｈの中心Ｃ０に対して所定の距離Ｙとなったときのロボットハンドの位置（座標および姿勢）である。このような状態となるときのロボットの各リンクの角度を算出する。
なお、ロボットハンド１２が挿入手前位置にあるときのチューブＴの先端Ｅの座標は、孔Ｈ近辺となるのが好ましい。例えば、チューブＴの先端Ｅが孔Ｈの中心Ｃ０に対する距離Ｙは、５～１００ｍｍ、好ましくは７ｍｍ～７０ｍｍ、特に好ましくは１０～５０ｍｍとなる位置が挙げられる。
第２撮像位置から挿入手前位置までの軌跡は、特に限定されるものではなく、好ましくは直線補間、円弧補間、関節補間等で算出する。
【0030】
ロボットハンド１２を挿入位置まで移動させる（工程９）。
工程９は、ロボットハンドの挿入位置を算出し、ロボットハンド１２を挿入手前位置から挿入位置まで移動させる。
ロボットハンド１２の挿入位置は、図４ｂに示すように、ロボットハンドがチューブＴを把持した状態で、チューブＴの先端Ｅの向きが孔Ｈの中心軸Ｃと一致し、かつ、チューブＴの先端Ｅが孔Ｈの内部にあるときのロボットハンドの位置（座標および姿勢）である。ここで、先端Ｅと孔Ｈの中心Ｃ０との距離Ｙ１は、０～２０ｍｍ、好ましくは１～１５ｍｍ、特に好ましくは２～１０ｍｍである。挿入位置における先端Ｅの孔Ｈの中心Ｃ０からの距離Ｙ１は、取り扱うチューブＴの湾曲度やチューブＴの孔Ｈに対するクリアランスに応じて適宜選択する。
つまり、工程８の挿入手前位置（図４ａ）において、チューブＴの先端Ｅの向きＤ２が孔Ｈの中心軸Ｃと実質的に一致し、かつ、そのチューブＴの先端Ｅが孔Ｈの中心Ｃ０に対して所定の距離Ｙとなっているため、工程９では、孔Ｈの中心Ｃ０から所定の距離Ｙだけ離れているチューブＴの先端が孔Ｈに挿入されるまでチューブＴを平行移動させる。それによりチューブＴの先端を真っ直ぐ孔に挿入できる。
このようにロボットハンド１２を挿入位置に移動させることにより、少なくともチューブＴの先端Ｅが孔Ｈに挿入される。
工程９は、ロボットハンドの挿入位置を算出し、ロボットハンド１２を挿入手前位置から挿入位置まで移動させる。
ロボットハンド１２の挿入位置は、図４ｂに示すように、ロボットハンドがチューブＴを把持した状態で、チューブＴの先端Ｅの向きが孔Ｈの中心軸Ｃと一致し、かつ、チューブＴの先端Ｅが孔Ｈの内部にあるときのロボットハンドの位置（座標および姿勢）である。ここで、先端Ｅと孔Ｈの中心Ｃ０との距離Ｙ１は、０～２０ｍｍ、好ましくは１～１５ｍｍ、特に好ましくは２～１０ｍｍである。挿入位置における先端Ｅの孔Ｈの中心Ｃ０からの距離Ｙ１は、取り扱うチューブＴの湾曲度やチューブＴの孔Ｈに対するクリアランスに応じて適宜選択する。
つまり、工程８の挿入手前位置（図４ａ）において、チューブＴの先端Ｅの向きＤ２が孔Ｈの中心軸Ｃと実質的に一致し、かつ、そのチューブＴの先端Ｅが孔Ｈの中心Ｃ０に対して所定の距離Ｙとなっているため、工程９では、孔Ｈの中心Ｃ０から所定の距離Ｙだけ離れているチューブＴの先端が孔Ｈに挿入されるまでチューブＴを平行移動させる。それによりチューブＴの先端を真っ直ぐ孔に挿入できる。
このようにロボットハンド１２を挿入位置に移動させることにより、少なくともチューブＴの先端Ｅが孔Ｈに挿入される。
【0031】
チューブＴを孔に押し込みながらロボットハンド１２を挿入完了位置まで移動させる（工程１０）。
工程１０は、ロボットハンド１２の挿入完了位置を算出し、ロボットハンド１２を挿入位置から挿入完了位置まで移動させながらチューブＴを孔に押し込み、チューブＴの挿入を完了させる。
ロボットハンドの挿入完了位置は、図５ａに示すように、ロボットハンドがチューブＴを略垂直に把持した状態で、ロボットハンドのフィンガ１２ａが孔Ｈの中心軸Ｃと略垂直であり、かつ、ロボットハンドのフィンガ１２ａの中心Ｏ（チューブＴの把持された部分Ｇの中心）と孔Ｈの中心Ｃ０との距離Ｚにあるときのロボットハンドの位置（座標および姿勢）である。なお、このロボットハンドの挿入完了位置は、ロボットハンドと孔Ｈの関係であるため、例えば、ロボットの各リンクの角度などによって、その座標および撮像姿勢を予め定めて制御装置に登録しておくのが好ましい。
工程１０は、ロボットハンド１２の挿入完了位置を算出し、ロボットハンド１２を挿入位置から挿入完了位置まで移動させながらチューブＴを孔に押し込み、チューブＴの挿入を完了させる。
ロボットハンドの挿入完了位置は、図５ａに示すように、ロボットハンドがチューブＴを略垂直に把持した状態で、ロボットハンドのフィンガ１２ａが孔Ｈの中心軸Ｃと略垂直であり、かつ、ロボットハンドのフィンガ１２ａの中心Ｏ（チューブＴの把持された部分Ｇの中心）と孔Ｈの中心Ｃ０との距離Ｚにあるときのロボットハンドの位置（座標および姿勢）である。なお、このロボットハンドの挿入完了位置は、ロボットハンドと孔Ｈの関係であるため、例えば、ロボットの各リンクの角度などによって、その座標および撮像姿勢を予め定めて制御装置に登録しておくのが好ましい。
【0032】
ロボットハンドがチューブＴを略垂直に把持していない状態の場合、挿入完了位置は、チューブＴの把持された部分Ｇの向きＤ１が孔Ｈの中心軸Ｃと実質的に一致し、かつ、把持された部分Ｇが孔Ｈの中心から所定の距離となるときのロボットハンドの位置（座標および姿勢）である。工程１で得られたチューブＴ三次元形状とロボットのチューブ把持方向とから、チューブＴの把持された部分の向きを取得することができる。このチューブの把持された部分の向きとロボットハンドの把持姿勢とから、挿入完了位置でのロボットハンドの姿勢（ロボットハンドのフィンガの向きＦ１）を算出することができる。
このようにロボットハンド１２を挿入完了位置まで移動させることにより、チューブＴの挿入部Ｉが湾曲していたとしても、孔の途中で引っかかることなく、実質的にチューブＴの被挿入部全体が孔に押し込まれる。
このようにロボットハンド１２を挿入完了位置まで移動させることにより、チューブＴの挿入部Ｉが湾曲していたとしても、孔の途中で引っかかることなく、実質的にチューブＴの被挿入部全体が孔に押し込まれる。
【0033】
ロボットハンドの挿入完了位置としては、例えば、ロボットハンドのフィンガ１２ａの中心Ｏ（チューブＴの把持された部分Ｇの中心）と孔Ｈの中心Ｃ０との距離Ｚが、０より大きく５０ｍｍ以下の位置、好ましくは０より大きく２０ｍｍ以下の位置、特に好ましくは０より大きく１０ｍｍ以下の位置が挙げられる。なお、ロボットハンドのフィンガ１２ａの中心Ｏと孔Ｈの中心Ｃ０の距離Ｚが０である場合、フィンガ１２ａと孔Ｈとは接触することになるため実用的ではない。ロボットハンドのフィンガ１２ａと孔Ｈの距離Ｚが５０ｍｍより大きいとチューブＴの把持工程から挿入完了までの間のチューブＴのハンドリング動作において、チューブＴの撓みの影響が不要に大きくなる。
【0034】
ロボットの挿入位置から挿入完了位置までの軌跡は、特に限定されるものではなく、上述したように直線補間、円弧補間、関節補間で算出する。特に関節補間によって算出させるのが好ましい。つまり、挿入位置のロボットの各リンクの関節角度と、挿入完了位置のロボットの各リンクの関節角度とを取得し、各リンクの関節角度差を計算し、それらを補間させて軌跡を決定する。これにより、図５ｂに示すように、ロボットハンド１２を、湾曲しているチューブＴの形状に沿うように孔Ｈの開口面に漸次近づかせながら挿入完了位置まで移動させることができる。つまり、湾曲したチューブＴの形状に近い軌跡を辿らせることができ、チューブＴが途中で折れたり、孔Ｈの途中でつっかえたりしにくい。しかし、ロボットの挿入位置から挿入完了位置までを円弧と近似し、その円弧を軌跡とするように円弧補間によってロボットハンド１２の軌跡を算出させてもよい。この場合、関節補間に比べて計算が煩雑になるが、一層湾曲しているチューブＴの形状に沿って移動させることができるため、チューブＴが非常に柔らかいとき、押し込み工程においてチューブＴが折れにくく好ましい。
このように、ロボットハンド１２を挿入完了位置においてチューブＴが把持された部分の向きＤ１が孔Ｈの中心軸Ｃと実質的に一致するように、挿入位置から移動させることにより、孔に挿入されたチューブＴの挿入部Ｉは、孔Ｈにガイドされて略直線状となり、把持された部分Ｇの近辺まで確実に挿入される。
このように、ロボットハンド１２を挿入完了位置においてチューブＴが把持された部分の向きＤ１が孔Ｈの中心軸Ｃと実質的に一致するように、挿入位置から移動させることにより、孔に挿入されたチューブＴの挿入部Ｉは、孔Ｈにガイドされて略直線状となり、把持された部分Ｇの近辺まで確実に挿入される。
【0035】
このように図２の線状物の挿入方法は、ロボットハンドを挿入手前位置に移動させているため、チューブＴの挿入操作が簡単にできる。またロボットハンドを挿入位置まで移動させているため、チューブＴの先端Ｅを確実に孔Ｈに挿入させることができる。そして、ロボットハンドを挿入完了位置まで移動させているため、挿入部Ｉの基端（チューブＴの把持される部分Ｇ）のぎりぎりまでロボットハンドを孔に接触させることなく挿入することができる。
【0036】
図２の線状物の挿入方法において、同じ材質で同じ形状の複数のチューブＴの挿入を行う場合、これらのチューブＴの湾曲度は実質的に同じになる。その場合、ロボットの第２撮像位置、ロボットの挿入手前位置、ロボットハンドの挿入位置、および、ロボットハンドの軌跡を予め制御装置３０に覚えさせてもよい。このようにロボットハンドに挿入位置および挿入完了位置を事前にティーチングすることにより、その後の線状物の挿入方法における工程５～工程８の計算を省略させることができる。
この場合も、ロボットハンドは挿入完了位置まで移動することになるので、上記の実施形態と同様の効果を奏することができる。
この場合も、ロボットハンドは挿入完了位置まで移動することになるので、上記の実施形態と同様の効果を奏することができる。
【0037】
上記の実施形態において、ロボットハンドを挿入手前位置まで移動させていたが、挿入手前位置を経由させず、把持位置または第２撮像位置から直接挿入位置に移動させてもよい。つまり、工程８を省略してもよい。
【0038】
上記の実施形態では、収納筒Ｓに収容された一本のチューブＴを把持し、孔Ｈに挿入したが、地面等に置かれたチューブＴを把持し、孔Ｈに挿入させてもよい。また、複数のチューブＴから一本のチューブを選択、把持して孔Ｈに挿入させてもよい。複数の線状物から一本を把持する方法としては、例えば、特許文献２に示すように、注目線状物を決定して把持させればよい。収納筒Ｓに複数のチューブＴを挿して、注目線状物を決定させてもよい。
【0039】
上記の実施形態では、可撓性を有する線状物を対象としているが、線状体の挿入部が非直線状であれば、可撓性を有さない硬い線状物であってもよい。この場合、初めにチューブＴの三次元形状を取得すれば、挿入部の三次元形状は取得しなくてもよい（工程５～７を省略）。また、上記の実施形態のチューブは、自重によって一方向に湾曲しているが、もともと曲がり癖がついている線状物も対象とすることができる。
【0040】
上記の実施形態では、チューブＴ（線状物）がある程度の硬度または弾性を有している、または、チューブＴの先端から把持する部分Ｇまでの距離が短く撓みが小さいため、問題としていないが、線状物が非常に柔らかい場合、工程５のロボットハンドの第２撮像位置における把持される部分Ｇの向きを、工程９のロボットハンドの挿入位置における把持される部分Ｇの向きと実質的に同じにするのが好ましい。つまり、線状物（チューブＴ）の湾曲度（撓み具合）はチューブＴの把持される部分Ｇの向きによって異なる。詳しくは、線状物（チューブＴ）の把持される部分Ｇが上を向いている場合、撓みが大きくなり、下向きに向かって撓みが小さくなる。そのため、工程５のロボットハンドの第２撮像位置における把持される部分Ｇの向きが、工程９のロボットハンドの挿入位置における把持される部分Ｇの向きと異なると、線状物の撓み形状（湾曲度）が異なる。結果、工程５の第２撮像位置において撮像した画像に基づいた線状物の先端の位置および向き（工程７）では、工程９において挿入できなくなる。このようにロボットハンドの第２撮像位置における把持される部分Ｇの向きを、工程９のロボットハンドの挿入位置における把持される部分Ｇの向きと実質的に同じにすることにより、非常に柔らかい線状物であっても正確に孔に挿入することができる。
なお、工程５の第２撮像位置におけるロボットハンドの向きは、例えば、予めサンプルのチューブを用いて適当な向きを登録したり、１本前の線状物の工程９で把持している向きを覚えさせたりする方法が挙げられる。
なお、工程５の第２撮像位置におけるロボットハンドの向きは、例えば、予めサンプルのチューブを用いて適当な向きを登録したり、１本前の線状物の工程９で把持している向きを覚えさせたりする方法が挙げられる。
【符号の説明】
【0041】
１ 線状物の孔挿入システム
１０ ロボット
１１ 多関節アーム
１２ ロボットハンド
１２ａ フィンガ
２０ 三次元計測装置
２１ 第１カメラ
２２ 第２カメラ
２３ カメラ制御部
３０ 制御装置
Ｃ 孔の中心軸
Ｅ 把持された状態のチューブ（挿入部）の先端
Ｇ 把持される部分
Ｈ 孔
Ｉ 挿入部
Ｓ 収納筒
Ｓｐ 鉛直面
Ｔ チューブ
１０ ロボット
１１ 多関節アーム
１２ ロボットハンド
１２ａ フィンガ
２０ 三次元計測装置
２１ 第１カメラ
２２ 第２カメラ
２３ カメラ制御部
３０ 制御装置
Ｃ 孔の中心軸
Ｅ 把持された状態のチューブ（挿入部）の先端
Ｇ 把持される部分
Ｈ 孔
Ｉ 挿入部
Ｓ 収納筒
Ｓｐ 鉛直面
Ｔ チューブ
【図1】
【図2】
【図3】
【図4】
【図5】