撮像面位置検出装置およびそれを具える作業ロボット
【課題】対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出する装置を提供することにある。
【解決手段】レーザ光軸が互いに交差するように配置された少なくとも３台のレーザ光照射手段と、前記少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像する１台の単眼の撮像手段と、前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の撮像光軸との位置関係から幾何学的演算により前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する演算手段と、を具えてなる、撮像面位置検出装置である。
【解決手段】レーザ光軸が互いに交差するように配置された少なくとも３台のレーザ光照射手段と、前記少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像する１台の単眼の撮像手段と、前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の撮像光軸との位置関係から幾何学的演算により前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する演算手段と、を具えてなる、撮像面位置検出装置である。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、ロボットの頭部やハンドに設けられたビデオカメラ等のカメラで撮像した対象物の撮像面の、撮像したカメラに対する位置すなわち距離と向きとを検出する装置並びに、その装置を具える作業ロボットに関するものである。
【背景技術】
【０００２】
近年、例えば１人ないし数人の作業者が１つの製品を作り上げるセル生産の現場等に、人と協調作業する作業ロボットを投入することが提案されており、かかる作業ロボットとしては従来例えば、下半身に台車を具え、その台車により作業場所内の床上を移動させることで、作業腕を持つ上半身をテーブル上等の作業領域の近傍の作業位置に簡便に配置し得るようにしたものが知られている（特許文献１参照）。
【０００３】
この作業ロボットではまた、頭部に複数台のビデオカメラを具え、それらのビデオカメラでテーブルやその上のワークに設けた複数のマークを複数方向から撮像して、それら複数方向からの複数枚の画像からビデオカメラひいては作業ロボットに対するワークやテーブルの位置すなわち距離と向きとを検出することで、上記のような簡便な配置でもワークの位置を作業ロボットに教示せずにワークに対する作業を行わせ得るようにしている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００７−１１８１７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上述のように、作業ロボットの利用を進めるために視覚技術を取り入れることは、視覚による情報量が他の知覚と比べて圧倒的に多いため極めて有効であり、視覚技術としてはカメラ画像を利用することが簡便である。
【０００６】
しかしながら、カメラ画像は２次元情報であるため、対象物の位置を３次元的に捉えるには複数枚のカメラ画像が必要となる。そして、特に作業ロボットの作動制御等のためにリアルタイム（実時間）で複数枚の画像を得るには、上述のように複数台のビデオカメラを具えることが必須となって、対象物位置検出装置の製造コストひいては作業ロボットの製造コストが嵩むことになるという問題がある。
【０００７】
また、複数枚の画像を処理してそれらの画像相互の関係から２次元情報を３次元情報に変換するには、複雑な処理と短くはない解析時間とを要するという問題もある。
【０００８】
それゆえこの発明は、１台の単眼の撮像手段で撮像した対象物の１枚の画像から撮像手段に対する対象物の撮像面の位置すなわち距離と向きとを検出することで、対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出する装置を提供することを目的としている。
【０００９】
またこの発明は、上述の装置を具える作業ロボットを提供することを目的としている。
【課題を解決するための手段】
【００１０】
この発明は上記課題を有利に解決するものであり、この発明の撮像面位置検出装置は、レーザ光軸が互いに交差するように配置された少なくとも３台のレーザ光照射手段と、前記少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像する１台の単眼すなわち単一光軸の撮像手段と、前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の撮像光軸との位置関係から幾何学的演算により前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する演算手段と、を具えてなるものである。
【００１１】
またこの発明の作業ロボットは、前記撮像手段に対する前記対象物の撮像面の距離と向きとを検出して出力する前記撮像面位置検出装置と、前記出力された撮像面の距離と向きとに基づき、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させる作動制御手段と、を具えてなるものである。
【発明の効果】
【００１２】
上述したこの発明の撮像面位置検出装置にあっては、少なくとも３台のレーザ光照射手段がそれぞれ、レーザ光軸が互いに交差する向きでレーザビームを対象物の表面に照射し、１台の単眼の撮像手段がそれら少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像し、演算手段が、前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の単一の撮像光軸との位置関係から幾何学的演算により、前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する。
【００１３】
従って、この発明の撮像面位置検出装置によれば、１台の単眼の撮像手段で撮像した対象物の１枚の画像から撮像手段に対する対象物の撮像面の位置すなわち距離と向きとを幾何学的演算により検出するので、対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出することができる。
【００１４】
なお、この発明の撮像面位置検出装置においては、前記少なくとも３台のレーザ光照射手段からのレーザビームは、前記撮像手段の撮像光軸上の１点で互いに交差すると好ましい。撮像光軸を基準にしてレーザビームの各照射点の位置を表せるので幾何学的計算をより簡易化できるからでる。
【００１５】
また、この発明の撮像面位置検出装置においては、前記レーザ光照射手段を奇数台具えていると好ましい。対象物の撮像面上のレーザビームの照射点の数が奇数となるため、対象物の撮像面がレーザ光軸の交差点より手前のある場合と先にある場合とで照射点を結んだ図形の向きが逆向きになるので、撮像面の位置をより容易に計算できるからである。
【００１６】
さらに、この発明の撮像面位置検出装置においては、前記対象物の撮像面は平面であると好ましい。対象物の撮像面上のレーザビームの照射点の位置関係から撮像面の位置をより容易に計算できるからである。
【００１７】
一方、上述したこの発明の作業ロボットにあっては、作業ロボットに設けられた撮像面位置検出装置が、その撮像手段に対する対象物の撮像面の距離と向きとを検出して出力し、作動制御手段が、撮像面位置検出装置から出力された前記撮像面の距離と向きとに基づき、その作業ロボットの腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させる。
【００１８】
従って、この発明の作業ロボットによれば、簡易かつ安価な構成により、実質的にリアルタイムで対象物の撮像面の位置を求めて、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させることができるので、製造コストを高めることなく作業ロボットの作動速度を高めることができる。
【００１９】
しかも、撮像面位置検出装置の少なくとも３台のレーザ光照射手段と撮像手段との位置関係が固定されているので、撮像面位置検出装置を具える頭や腕等の部位が作動停止後に多少振動していても、その振動にほとんど影響されずに撮像面の位置検出を行うことができる。
【図面の簡単な説明】
【００２０】
【図１】この発明の撮像面位置検出装置の一実施例の構成を、光学系を斜め後方から見た状態で示す構成図である。
【図２】上記実施例の撮像面位置検出装置の光学系を斜め前方から見た状態で示す斜視図である。
【図３】上記実施例の撮像面位置検出装置の光学系をカメラ画像の向きで見た状態で示す平面図である。
【図４】上記実施例の撮像面位置検出装置の光学系と対象物との幾何学的位置関係を図３中のＡ−Ａ線に沿う断面で示す説明図である。
【図５】上記実施例の撮像面位置検出装置の光学系と対象物との幾何学的位置関係を図４と同様の位置での断面で示す説明図である。
【図６】上記実施例の撮像面位置検出装置を具えた、この発明の作業ロボットの一実施例を示す斜視図である。
【図７】上記実施例の作業ロボットの腕の先端部に具えるハンドおよび上記実施例の撮像面位置検出装置を示す斜視図である。
【図８】図７に示すハンドおよび上記実施例の撮像面位置検出装置を斜め前方から見た状態で示す斜視図である。
【発明を実施するための形態】
【００２１】
以下、この発明の実施の形態を図面に基づく実施例によって詳細に説明する。ここに、図１は、この発明の撮像面位置検出装置の一実施例の構成を、光学系を斜め後方から見た状態で示す構成図、図２は、上記実施例の撮像面位置検出装置の光学系を斜め前方から見た状態で示す斜視図、図３は、上記実施例の撮像面位置検出装置の光学系をカメラ画像の向きで見た状態で示す平面図、そして図４は、上記実施例の撮像面位置検出装置の光学系と対象物との幾何学的位置関係を図３中のＡ−Ａ線に沿う断面で示す説明図である。
【００２２】
この実施例の撮像面位置検出装置は、レーザビーム（光線）の光軸が互いに交差するように配置された３台の、レーザ光照射手段としてのレーザ光照射装置１と、それら３台のレーザ光照射装置１に対する所定位置で、それらのレーザ光照射装置１からレーザビームＢ１，Ｂ２，Ｂ３を照射された対象物２の表面である撮像面２ａを撮像する１台の、単眼の撮像手段としてのビデオカメラ３と、そのビデオカメラ３が対象物（図示例では平板）２の撮像面２ａを撮像した１枚の画像での、３台のレーザ光照射装置１からのレーザビームＢ１，Ｂ２，Ｂ３の３つの照射点Ｉ１，Ｉ２，Ｉ３の位置とビデオカメラ３の撮像光軸としてのレンズ光軸ＬＣとの位置関係から幾何学的演算により、ビデオカメラ３に対する対象物２の撮像面２ａの距離と向きとを求めて出力する、演算手段としての演算装置４と、上記３台のレーザ光照射装置１と１台のビデオカメラ３とを互いに位置決めして固定支持するブラケット５と、を具えてなるものである。
【００２３】
ここで、レーザ光照射装置１としては、例えばレーザダイオードで出力したレーザビームを表示物等に照射し、そのレーザビームの照射点である光点で表示物等を指し示す通常のレーザポインタを用いることができ、またビデオカメラ３としては、例えばＣＣＤ撮像素子で対象物を撮像した画像を電気信号として出力する通常のビデオカメラを用いることができ、そして演算装置４としては、例えばＣＰＵ（中央処理ユニット）、ＲＯＭ（読み出し専用メモリ）、ＲＡＭ（ランダムアクセスメモリ）およびＩ／Ｏ（入出力インターフェース）等を持ち、予めＲＯＭ等に記憶したプログラムに基づき一時記憶にＲＡＭ等を用いながらＣＰＵで演算処理を行ってその結果をＩ／Ｏから出力する通常のマイクロコンピュータを用いることができる。
【００２４】
この実施例におけるブラケット５は、図３に示すように、枠状部５ａによりビデオカメラ３を固定支持するとともに、その枠状部５ａから放射状に突出した３本の腕部５ｂに図示しないボルトで位置調節可能に固定した筒状部５ｃにより３台のレーザ光照射装置１をそれぞれ固定支持している。かかる支持構造により、図４に１台のレーザ光照射装置１について代表で示すように、３台のレーザ光照射装置１からのレーザビームＢ１，Ｂ２，Ｂ３（図４では符号Ｂで示す）の光軸はそれぞれ、ビデオカメラ３のレンズ光軸ＬＣに対しレンズ３ａの位置から距離Ｈｍｍの１点ＣＰで交差するとともに互いに同一の角度θをなし、また図３の平面図で真上から見て示すように、それらのレーザビームＢ１，Ｂ２，Ｂ３のそれぞれの光軸とビデオカメラ３のレンズ光軸ＬＣとを含む、図３の紙面に垂直な平面が３枚存在し、それらの垂直平面はレンズ光軸ＬＣを交線として互いに１２０°の角度で交差している。
【００２５】
上記の位置関係において演算装置４は、ビデオカメラ３が撮像した１枚の画像から、ビデオカメラ３に対する対象物２の、図示例では平面である撮像面２ａの３次元的な距離と向きとを、ビデオカメラ３と撮像面２ａとの幾何学的な関係に基づき、以下の如くして演算で求める。
【００２６】
すなわち、図３に示す向きで撮像したビデオカメラ３の画像は、四角の１コマを１ピクセルとすると水平ピクセル数と垂直ピクセル数とで規定され、指定するピクセルの位置は２次元座標（ｘ，ｙ）にて表記できる。ここで、画像上でのレンズ光軸ＬＣの位置を画像原点（０，０）とするとともに、その画像原点（０，０）からレーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３までの距離をａ１，ａ２，ａ３とすると、
レーザビームＢ１の照射点Ｉ１の位置は（ａ１，０）、
レーザビームＢ２の照射点Ｉ２の位置は（ａ２・cos120°，ａ２・sin120°）、
レーザビームＢ３の照射点Ｉ３の位置は（ａ３・cos240°，ａ３・sin240°）であり、
照射点Ｉ１，Ｉ２，Ｉ３は、画像原点（０，０）から０°、１２０°、２４０°の角度を通る直線上にそれぞれ位置する。
【００２７】
対象物２の撮像面２ａの３次元的な向きを計算すると、図４に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの手前（ビデオカメラ３寄り）に対象物２の撮像面２ａがある場合には、その交点ＣＰを三次元座標系の原点０（０，０，０）、レンズ光軸ＬＣを三次元座標系のｚ軸方向としたときの、撮像面２ａの向きを表す、原点０から撮像面２ａへ向かう単位長さの法線ベクトルＮＵ→（ｌｘ，ｌｙ，ｌｚ）の各座標軸成分は、
照射点Ｉ２の長さ比ｒ２＝ａ２／ａ１、
照射点Ｉ３の長さ比ｒ３＝ａ３／ａ１より、
ｌｘ＝ｋ（１／ｒ２＋１／ｒ３）／（√３tanθ）、
ｌｙ＝ｋ（２−１／ｒ２＋１／ｒ３）／（３tanθ）、
ｌｚ＝ｋ（１−（２−１／ｒ２＋１／ｒ３）／３）
となる。
ここで、ｋ＝１／√｛（１／ｒ２＋１／ｒ３）／（√３tanθ）２＋（２−１／ｒ２＋１／ｒ３）／（３tanθ）２＋（１−（２−１／ｒ２＋１／ｒ３）／３）２｝である。
【００２８】
これについて説明すると、ある点Ａ＝（ｘ０，ｙ０，ｚ０）を通り、その点Ａおよび原点０（０，０，０）を通る直線に垂直な平面を考えた場合に、点Ａからその平面上の任意の点Ｐ（Ｘ，Ｙ，Ｚ）に引いたベクトルＡＰ→は、
ＡＰ→＝（Ｘ−ｘ０，Ｙ−ｙ０，Ｚ−ｚ０）であり、
原点Ｏから点Ａに引いたベクトルＯＡ→は、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、それら互いに直角な２つのベクトルＡＰ→，ＯＡ→の内積は０であることから、
ｘ０（Ｘ−ｘ０）＋ｙ０（Ｙ−ｙ０）＋ｚ０（Ｚ−ｚ０）＝０
が成り立つ。
【００２９】
これをこの実施例の装置に当てはめると、
（１）レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰに設定した原点Ｏ（０，０，０）からレーザビームＢ１上を照射点Ｉ１へ向かう所定長さのベクトルｂ１→を考えると、ｂ１→が３次元座標系のｙ軸と、レンズ光軸ＬＣに一致するｚ軸とを含む平面（先の３枚の垂直平面の１つ）の上にあるとして、
ｂ１→＝（ｂ１ｘ，ｂ１ｙ，ｂ１ｚ）＝（sin0°・tanθ，cos0°・tanθ，１）＝（０，tanθ，１）となり、ベクトルｂ１→の長さは
１／cosθ＝√（１＋tan２θ）=√｛（cos２θ＋sin２θ）／cos２θ｝=√（１／cos２θ）
となる。
上記ベクトルｂ１→を含む直線であるレーザビームＢ１の光軸上の点の座標は、ｔを変数とすると、
ｘ＝sin0°・tanθ・ｔ＝０
ｙ＝cos0°・tanθ・ｔ＝tanθ・ｔ
ｚ＝１・ｔ＝ｔ
となる。
【００３０】
上記点Ａ（ｘ０，ｙ０，ｚ０）が、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を含む平面である撮像面２ａ上にあると考えると、その点Ａおよび原点０（０，０，０）を通る直線は撮像面２ａの法線となるので、上記ベクトルｂ１→を含む直線であるレーザビームＢ１の光軸と、上記法線に垂直な平面である撮像面２ａとの交点であるレーザビームＢ１の照射点Ｉ１を点Ｑ（ｘ，ｙ，ｚ）とすると、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、
ＡＱ→＝（０−ｘ０，tanθ・ｔ−ｙ０，ｔ−ｚ０）である。
これら互いに直角な２つのベクトルＡＱ→，ＯＡ→の内積は０であることから、
ｘ０（０−ｘ０）＋ｙ０（tanθ・ｔ−ｙ０）＋ｚ０（ｔ−ｚ０）＝０
となり、これをｔについて整理すると、
（ｙ０・tanθ＋ｚ０）ｔ＝ｘ０２＋ｙ０２＋ｚｏ２＝Ｌ２
となる。このＬは、原点０（０，０，０）すなわちレーザビームＢ１とレンズ光軸ＬＣとの交点ＣＰから撮像面２ａまでの距離（法線上の長さ）を表す。
また、ｔ＝Ｌ２／（ｙ０・tanθ＋ｚ０）となる。
【００３１】
（２）原点Ｏ（０，０，０）からレーザビームＢ２上を照射点Ｉ２へ向かう所定長さのベクトルｂ２→を考えると、ｂ２→が、３次元座標系のｙ軸とｚ軸とを含む平面に対しｚ軸を中心として反時計回りに１２０°回転した位置にある平面（先の３枚の鉛直平面の他の１つ）の上にあるとして、
ｂ２→＝（ｂ２ｘ，ｂ２ｙ，ｂ２ｚ）＝（sin120°・tanθ，cos120°・tanθ，１）となり、上記ベクトルｂ２→を含む直線であるレーザビームＢ２の光軸上の点の座標は、ｔを変数とすると、
ｘ＝sin120°・tanθ・ｔ
ｙ＝cos120°・tanθ・ｔ
ｚ＝１・ｔ＝ｔ
となる。
【００３２】
上記のように点Ａ（ｘ０，ｙ０，ｚ０）が、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を含む平面である撮像面２ａ上にあると考えると、その点Ａおよび原点０（０，０，０）を通る直線は撮像面２ａの法線となるので、上記ベクトルｂ２→を含む直線であるレーザビームＢ２の光軸と、上記法線に垂直な平面である撮像面２ａとの交点であるレーザビームＢ２の照射点Ｉ２を点Ｒ（ｘ，ｙ，ｚ）とすると、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、
ＡＲ→＝（sin120°・tanθ・ｔ−ｘ０，cos120°・tanθ・ｔ−ｙ０，ｔ−ｚ０）である。
これら互いに直角な２つのベクトルＡＲ→，ＯＡ→の内積は０であることから、
ｘ０（sin120°・tanθ・ｔ−ｘ０）＋ｙ０（cos120°・tanθ・ｔ−ｙ０）＋ｚ０（ｔ−ｚ０）＝０
となり、これをｔについて整理すると、
（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０）ｔ＝Ｌ２
となる。
また、ｔ＝Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０）となる。
【００３３】
（３）原点ＯからレーザビームＢ３上を照射点Ｉ３へ向かう所定長さのベクトルｂ３→を考えると、ｂ３→が、３次元座標系のｙ軸とｚ軸とを含む平面に対しｚ軸を中心として反時計回りに２４０°回転した位置にある平面（先の３枚の鉛直平面の他の１つ）の上にあるとして、
ｂ３→＝（ｂ３ｘ，ｂ３ｙ，ｂ３ｚ）＝（sin240°・tanθ，cos240°・tanθ，１）となり、上記ベクトルｂ３→を含む直線であるレーザビームＢ３の光軸上の点の座標は、ｔを変数とすると、
ｘ＝sin240°・tanθ・ｔ
ｙ＝cos240°・tanθ・ｔ
ｚ＝１・ｔ＝ｔ
となる。
【００３４】
上記のように点Ａ（ｘ０，ｙ０，ｚ０）が、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を含む平面である撮像面２ａ上にあると考えると、その点Ａおよび原点０（０，０，０）を通る直線は撮像面２ａの法線となることから、上記ベクトルｂ３→を含む直線であるレーザビームＢ３の光軸と、上記法線に垂直な平面である撮像面２ａとの交点であるレーザビームＢ２の照射点Ｉ２を点Ｓ（ｘ，ｙ，ｚ）とすると、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、
ＡＳ→＝（sin240°・tanθ・ｔ−ｘ０，cos240°・tanθ・ｔ−ｙ０，ｔ−ｚ０）である。
これら互いに直角な２つのベクトルＡＳ→，ＯＡ→の内積は０であることから、
ｘ０（sin240°・tanθ・ｔ−ｘ０）＋ｙ０（cos240°・tanθ・ｔ−ｙ０）＋ｚ０（ｔ−ｚ０）＝０
となり、これをｔについて整理すると、
（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０）ｔ＝Ｌ２
となる。
また、ｔ＝Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０）となる。
【００３５】
（４）レーザビームＢ１の照射点Ｉ１のｘｙ平面座標は、上記（１）からｘ＝０、ｙ＝tanθ・ｔであり、ｔ＝Ｌ２／（ｙ０・tanθ＋ｚ０）であるので、
（０，tanθ・Ｌ２／（ｙ０・tanθ＋ｚ０））となる。
またレーザビームＢ２の照射点Ｉ２のｘｙ平面座標は、上記（２）からｘ＝sin120°・tanθ・ｔ、ｙ＝cos120°・tanθ・ｔであり、ｔ＝Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０）であるので、
（sin120°・tanθ・Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０），cos120°・tanθ・Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０））となる。
さらにレーザビームＢ３の照射点Ｉ３のｘｙ平面座標は、上記（３）からｘ＝sin240°・tanθ・ｔ、ｙ＝cos240°・tanθ・ｔであり、ｔ＝Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０）であるので、
（sin240°・tanθ・Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０），cos240°・tanθ・Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０））となる。
【００３６】
（５）撮像面２ａの向きを表す、原点０から撮像面２ａに至る法線ベクトルＮＬ→は（Ｌｘ，Ｌｙ，Ｌｚ）であるので、ｘ０＝Ｌｘ、ｙ０＝Ｌｙ、ｚ０＝Ｌｚであり、これを用いると、
レーザビームＢ１の照射点Ｉ１のｘｙ平面座標は、
（０，tanθ・Ｌ２／（Ｌｙ・tanθ＋Ｌｚ））となる。
またレーザビームＢ２の照射点Ｉ２のｘｙ平面座標は、sin120°＝√３／２，cos120°＝−１／２であるので、
（√３／２・tanθ・Ｌ２／（（√３・Ｌｘ−Ｌｙ）／２・tanθ＋Ｌｚ），−１／２・tanθ・Ｌ２／（（√３・Ｌｘ−Ｌｙ）／２・tanθ＋Ｌｚ））となる。
さらにレーザビームＢ３の照射点Ｉ３のｘｙ平面座標は、sin240°＝−√３／２，cos240°＝−１／２であるので、
（√３／２・tanθ・Ｌ２／（（√３・Ｌｘ＋Ｌｙ）／２・tanθ−Ｌｚ），１／２・tanθ・Ｌ２／（（√３・Ｌｘ＋Ｌｙ）／２・tanθ−Ｌｚ））となる。
【００３７】
（６）レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３の、ｘｙ平面座標系における原点（０，０）からの長さａ１，ａ２，ａ３は、単位長さの法線ベクトルＮＵ→（ｌｘ，ｌｙ，ｌｚ）（但しｌｘ２＋ｌｙ２＋ｌｚ２＝１）を用いて、Ｌｘ／Ｌ＝ｌｘ，Ｌｙ／Ｌ＝ｌｙ，Ｌｚ／Ｌ＝ｌｚとすると、
ａ１＝tanθ・Ｌ／（Ｌｙ／Ｌ・tanθ＋Ｌｚ／Ｌ）＝tanθ・Ｌ／（Ｌｙ／Ｌ・tanθ＋Ｌｚ／Ｌ）＝tanθ・Ｌ／（ｌｙ・tanθ＋ｌｚ）
ａ２＝tanθ・Ｌ／（（√３Ｌｘ／Ｌ−Ｌｙ／Ｌ）／２・tanθ＋Ｌｚ／Ｌ）＝tanθ・Ｌ／（（√３ｌｘ−ｌｙ）／２・tanθ＋ｌｚ）
ａ３＝tanθ・Ｌ／（（√３Ｌｘ／Ｌ＋Ｌｙ／Ｌ）／２・tanθ−Ｌｚ／Ｌ）＝tanθ・Ｌ／（（√３ｌｘ＋ｌｙ）／２・tanθ−ｌｚ）
となる。
【００３８】
（７）ここで、ａ１，ａ２，ａ３についての撮像面２ａの傾きに関する補正係数ｋを、
１≡ｋ・１／（ｌｙ・tanθ＋ｌｚ）と設定すると、
ｌｙ・tanθ＋ｌｚ＝ｋ
従って、ｌｚ＝ｋ−ｌｙ・tanθ
長さ比ｒ２＝ａ２／ａ１とすると、
（ｌｙ・tanθ＋ｌｚ）／（（√３ｌｘ−ｌｙ）／２・tanθ＋ｌｚ）＝ｒ２
これに上記のｌｚの式を入れると、
ｒ２・（（√３ｌｘ−ｌｙ）／２・tanθ＋ｌｚ）＝ｋ
さらに上記のｌｚの式を入れると、
ｒ２・（（√３ｌｘ−ｌｙ）／２・tanθ＋ｋ−ｌｙ・tanθ）＝ｋ
従って、ｒ２・（（√３ｌｘ−３ｌｙ）／２・tanθ＋ｋ）＝ｋとなる。
また、長さ比ｒ３＝ａ３／ａ１とすると、
（ｌｙ・tanθ＋ｌｚ）／（（√３ｌｘ＋ｌｙ）／２・tanθ−ｌｚ）＝ｒ３
これに上記のｌｚの式を入れると、
ｒ３・（（√３ｌｘ＋ｌｙ）／２・tanθ−ｌｚ）＝ｋ
さらに上記のｌｚの式を入れると、
ｒ３・（（√３ｌｘ＋ｌｙ）／２・tanθ−ｋ＋ｌｙ・tanθ）＝ｋ
従って、ｒ３・（（√３ｌｘ＋３ｌｙ）／２・tanθ−ｋ）＝ｋとなる。
【００３９】
これらから、
（√３ｌｘ−３ｌｙ）／２・tanθ＋ｋ−ｋ／ｒ２＝０
（√３ｌｘ＋３ｌｙ）／２・tanθ−ｋ−ｋ／ｒ３＝０
上記両式を加算すると、
√３ｌｘ・tanθ−ｋ／ｒ２−ｋ／ｒ３＝０
これより、ｌｘ＝ｋ（１／ｒ２＋１／ｒ３）／（√３tanθ）となる。
両式を減算すると、
ｋ（２−１／ｒ２＋１／ｒ３）＝３ｌｙ・tanθ
これより、ｌｙ＝ｋ（２−１／ｒ２＋１／ｒ３）／（３tanθ）となる。
さらに、上記のｌｚの式とｌｙより、
ｌｚ＝ｋ−ｌｙ・tanθ＝ｋ−｛ｋ（２−１／ｒ２＋１／ｒ３）／（３tanθ）｝・tanθ
＝ｋ−ｋ（２−１／ｒ２＋１／ｒ３）／３となる。
ここで、ｌｘ２＋ｌｙ２＋ｌｚ２＝１であるので、上記のｌｘ，ｌｙ，ｌｚを代入して、
ｋ２｛（１／ｒ２＋１／ｒ３）／（√３tanθ）２＋（２−１／ｒ２＋１／ｒ３）／（３tanθ）２＋（１−（２−１／ｒ２＋１／ｒ３）／３）２｝＝１
従って、ｋ＝１／√｛（１／ｒ２＋１／ｒ３）／（√３tanθ）２＋（２−１／ｒ２＋１／ｒ３）／（３tanθ）２＋（１−（２−１／ｒ２＋１／ｒ３）／３）２｝となる。
【００４０】
すなわち、撮像面２ａの傾きを表すｌｘ，ｌｙ，ｌｚおよび撮像面２ａの傾きに関する補正係数ｋは何れも、画像におけるａ１に対するａ２，ａ３の長さ比ｒ２，ｒ３と、あらかじめ定めたカメラ光軸ＬＣに対するレーザビームの角度θとから求めることができる。
【００４１】
なお、図５に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの先に対象物２の撮像面２ａがある場合にも、同様にして、撮像面２ａの傾きを表すｌｘ，ｌｙ，ｌｚおよび撮像面２ａの傾きに関する補正係数ｋを何れも、画像におけるａ１に対するａ２，ａ３の長さ比ｒ２，ｒ３と、あらかじめ定めたカメラ光軸ＬＣに対するレーザビームの角度θとから求めることができる。
【００４２】
次に、ビデオカメラ３のレンズ３ａから対象物２の撮像面２ａまでのカメラ光軸ＬＣ上での距離Ｄを計算すると、上述のように、図３に示す向きで撮像したビデオカメラ３の画像は、四角の１コマを１ピクセルとすると水平ピクセル数と垂直ピクセル数とで規定され、指定するピクセルの位置は２次元座標（ｘ，ｙ）にて表記できる。
ここで、既知の１ピクセルのサイズをｓ（ｍｍ）、画像上で求まるカメラ光軸ＬＣからレーザビームＢ１の照射点Ｉ１までのピクセル数をｎｐｉｃとすると、照射点Ｉ１の結像サイズＢｓは、
Ｂｓ＝ｓ・ｎｐｉｃ
で求まる。
そして、既知の焦点距離ｆ（ｍｍ）より結像画角αは、図４および図５に示すように、
ｔａｎα＝Ｂｓ／ｆ＝ａ／Ｄ
で求まる。
【００４３】
レーザビームＢ１において、撮像面２ａ上の照射点Ｉ１とカメラ光軸ＬＣとの距離ａ’（ａ’＝ａ１）は、図４に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの手前に対象物２の撮像面２ａがある場合（Ｄ＜Ｈ：但しＨは、ビデオカメラ３のレンズ３ａから、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とカメラ光軸ＬＣとの交点ＣＰまでの距離を表す）には、
ａ＝ａ’（１＋ｔａｎεｔａｎα）＝Ｄｔａｎα
で表される。但し、ｔａｎε＝Ｌｘ／Ｌｚ＝−ｌｘ／ｌｚである。
両片にｔａｎεを乗じると
ａ’（１＋ｔａｎεｔａｎα）ｔａｎε＝Ｄｔａｎαｔａｎε
従って、
ａ’ｔａｎε＝Ｄｔａｎαｔａｎε／（１＋ｔａｎεｔａｎα）
ここで、ｅ＝ｔａｎαｔａｎε／（１＋ｔａｎεｔａｎα）とすると、
ａ’ｔａｎε＝Ｄｅ
となる。
【００４４】
撮像面２ａ上の照射点Ｉ１の、レンズ３ａからのカメラ光軸ＬＣ上での距離は、図４に示すように、
Ｄ−ａ’ｔａｎε
であり、
一方、撮像面２ａ上の照射点Ｉ１の、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とレンズ光軸ＬＣとの交点ＣＰからのカメラ光軸ＬＣ上での距離は、
Ｌ／ｃｏｓζ＋ａ’ｔａｎε
である。但し、ｔａｎζ＝√（ｌｘ２＋ｌｙ２）／ｌｚである。
次に、ａ’と上記αおよび既知のレーザビーム角θとの関係を求めると、図４に示すように、
ａ’／（Ｄ−ａ’ｔａｎε）＝ｔａｎα
ａ’／（Ｌ／ｃｏｓζ＋ａ’ｔａｎε）＝ｔａｎθ
であり、これにより、
（Ｄ−ａ’ｔａｎε）ｔａｎα＝（Ｌ／ｃｏｓζ＋ａ’ｔａｎε）ｔａｎθ
ここで上記ａ’ｔａｎε＝Ｄｅを用いると、
（Ｄ−Ｄｅ）ｔａｎα＝（Ｌ／ｃｏｓζ＋Ｄｅ）ｔａｎθ
【００４５】
さらに、レンズ３ａから、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とレンズ光軸ＬＣとの交点ＣＰまでの距離Ｈは、図４に示すように、
Ｄ＋Ｌ／ｃｏｓζ＝Ｈ
従って、
Ｌ／ｃｏｓζ＝Ｈ−Ｄ
であり、先のａ’とαおよびθとの関係式に上記の式を代入してＬ／ｃｏｓζを消去すると、
（Ｄ−Ｄｅ）ｔａｎα＝（Ｈ−Ｄ＋Ｄｅ）ｔａｎθ
（Ｄ−Ｄｅ）で纏めると、
（Ｄ−Ｄｅ）（ｔａｎα＋ｔａｎθ）＝Ｈｔａｎθ
従って、
Ｄ（１−ｅ）（ｔａｎα＋ｔａｎθ）＝Ｈｔａｎθ
よって、ビデオカメラ３のレンズ３ａから対象物２の撮像面２ａまでのカメラ光軸ＬＣ上での距離Ｄは、
Ｄ＝Ｈｔａｎθ／｛（１−ｅ）（ｔａｎα＋ｔａｎθ）｝
となる。但し、ｅ＝ｔａｎαｔａｎε／（１＋ｔａｎεｔａｎα）である。
【００４６】
一方、図５に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの先に対象物２の撮像面２ａがある場合（Ｄ＞Ｈ：但しＨは、ビデオカメラ３のレンズ３ａから、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とカメラ光軸ＬＣとの交点ＣＰまでの距離を表す）には、
Ｌ／ｃｏｓζ＝Ｄ−Ｈ
であるから、レーザビームＢ１において、撮像面２ａ上の照射点Ｉ１とカメラ光軸ＬＣとの距離ａ’（ａ’＝ａ１）は、
ａ＝ａ’（１−ｔａｎεｔａｎα）＝Ｄｔａｎα
であり、ここで、ｇ＝ｔａｎαｔａｎε／（１−ｔａｎεｔａｎα）とすると、
ａ’ｔａｎε＝Ｄｇ
となる。
【００４７】
次に、Ｄ＜Ｈの場合と同様にしてａ’とαおよびθとの関係を求めると、図５に示すように、
ａ’／（Ｄ＋ａ’ｔａｎε）＝ｔａｎα
ａ’／（Ｄ−Ｈ＋ａ’ｔａｎε）＝ｔａｎθ
であり、これにより、
（Ｄ＋ａ’ｔａｎε）ｔａｎα＝（Ｄ−Ｈ＋ａ’ｔａｎε）ｔａｎθ
ここで上記ａ’ｔａｎε＝Ｄｇを用いると、
（Ｄ＋Ｄｇ）ｔａｎα＝（Ｄ−Ｈ＋Ｄｇ）ｔａｎθ
Ｄで纏めると、
Ｄ（１＋ｇ）ｔａｎα＝Ｄ（１−ｇ）ｔａｎθ−Ｈｔａｎθ
従って、
Ｄ｛（１−ｇ）ｔａｎθ−（１＋ｇ）ｔａｎα｝＝Ｈｔａｎθ
よって、ビデオカメラ３のレンズ３ａから対象物２の撮像面２ａまでのカメラ光軸ＬＣ上での距離Ｄは、
Ｄ＝Ｈｔａｎθ／｛（１−ｇ）ｔａｎθ−（１＋ｇ）ｔａｎα｝
＝Ｈｔａｎθ／｛ｔａｎθ−ｔａｎα−ｇ（ｔａｎθ＋ｔａｎα）｝
となる。但し、ｇ＝ｔａｎαｔａｎε／（１−ｔａｎεｔａｎα）である。
【００４８】
なお、対象物２の撮像面２ａが、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの手前にあるか先にあるかの判別は、ビデオカメラ３の画像における、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を結んだ三角形の向き（図３では、交点ＣＰの手前にあるので三角形の頂点が右に向いており、交点ＣＰの先にある場合は三角形の頂点が左に向く）を画像処理で識別すれば、容易に行うことができる。
【００４９】
そしてこのことは、レーザビームの本数が３本でなくても、その本数が奇数であるか、あるいはレーザビームの照射点を結んだ図形が特定の線に対して線対称（例えば正方形や長方形等）でないものであれば、同様にして成立する。
【００５０】
図６は、上記実施例の撮像面位置検出装置を具えた、この発明の作業ロボットの一実施例としての台車型作業ロボットを示す斜視図、図７は、上記実施例の台車型作業ロボットの腕の先端部に具えるハンドおよび上記実施例の撮像面位置検出装置を示す斜視図、そして図８は、図７に示すハンドおよび上記実施例の撮像面位置検出装置を斜め前方から見た状態で示す斜視図である。
【００５１】
この実施例の台車型作業ロボット１０は、撮像手段としての２台のテレビカメラ１１を有する頭部１２と、２本の作業腕１３と、それらの作業腕１３を支持する胴部１４とを有する上半身１５と、その上半身１５の胴部１４の上端部から下方へ延在する支持部材としての支柱１６とを具えている。２台のテレビカメラ１１は互いに略同一方向へ向いて一緒に頭部１２に固定され、その頭部１２は、頭部１２を水平なピッチ軸線Ｐ１周りに揺動させる首ピッチ軸駆動機構と、その首ピッチ軸駆動機構を鉛直なヨー軸線Ｙ１周りに回動させる首ヨー軸駆動機構とを有する２自由度の首部関節１７を介して胴部１４の上端部に取り付けられている。そして胴部１４の下端部は、ヨー軸線Ｙ１周りに胴部１４を回動させる胴部ヨー軸駆動機構を有する１自由度の胴部関節１８を介して支柱１６の下端部に取り付けられ、これにより支柱１６はその上端部で上半身１５を支持している。
【００５２】
また、２本の作業腕１３は胴部１４の左右両側にそれぞれ設けられ、各作業腕１３は、胴部１４と作業腕１３の上腕１３ａとの間の肩関節１９と、上腕１３ａと下腕１３ｂとの間の肘関節２０と、下腕１３ｂと図６〜８に示すハンド２１や図示しない電動ドライバ等のエンドエフェクタとの間の手首関節２２とを有しており、肩関節１９は、図６に示すように、胴部１４の左右に突出した肩ブラケット２３にそれぞれ配置され、胴部１４に対し作業腕１３の全体をヨー軸線Ｙ２周りに相対的に左右に回動させる肩ヨー軸駆動機構と、胴部１４に対し、そのヨー軸線Ｙ２と直交するピッチ軸線Ｐ２周りに作業腕１３の全体を相対的に前後に傾動させる肩ピッチ軸駆動機構とを有している。
【００５３】
ここで、肩ブラケット２３の肩ヨー軸駆動機構支持面は、鉛直なヨー軸線Ｙ１に対して斜め下方に１５度傾いて配置されており、このため各ヨー軸線Ｙ２は、胴部１４の上下方向に対して１５度傾いて、下方へ行くほど胴部１４に近くなるように延在し、それに伴い、各ヨー軸線Ｙ２と直交する各ピッチ軸線Ｐ２も、床面に対して１５度傾いている。そして作業腕１３の上腕１３ａは、下に下げた状態で肘関節２０に近い部分ほど胴部１４に近くなるように傾いて延在している。この構成によれば、この実施例の台車型作業ロボット１０がその横に位置する作業者と協調あるいは協働して作業を行う場合に、作業者の横に位置する肘関節２０が、作業者から離れており、しかも横に動かないので、作業者が作業腕１３と干渉する可能性を小さくすることができる。
【００５４】
さらに、各肘関節２０は、上腕１３ａに対し下腕１３ｂを、ピッチ軸線Ｐ２に平行なピッチ軸線Ｐ３周りに上下に傾動させる肘ピッチ軸駆動機構を有し、また、各手首関節２２は、下腕１３ｂに対してハンド２１や電動ドライバ等のエンドエフェクタをピッチ軸線Ｐ４周りに相対的に上下に傾動させる手首ピッチ軸駆動機構と、そのエンドエフェクタをピッチ軸線Ｐ４と直交するヨー軸線Ｙ３周りに相対的に左右に回動させる手首ヨー軸駆動機構と、下腕１３ｂに対して手首ピッチ軸駆動機構および手首ヨー軸駆動機構をその下腕１３ｂに沿って延在するロール軸線Ｒ周りにねじる手首ロール軸駆動機構とを有している。この結果として各作業腕１３は６自由度を有し、これらピッチ軸、ヨー軸およびロール軸駆動機構の軸配置により、２本の作業腕１３は特異点がなく自由な姿勢を作ることができる。なお、上記各関節のピッチ軸、ヨー軸およびロール軸の軸駆動機構は各々、周知のように例えばサーボモータ等のモータの出力回転を例えば商品名ハーモニックドライブ等の減速機で減速して出力する、逆入力可能な回動機構で構成されている。
【００５５】
そして、この実施例の台車型作業ロボット１０は、図６〜８に示すように、左の作業腕１３の手首関節２２で、エンドエフェクタとしてのハンド２１とともに、上記実施例の撮像面位置検出装置の１台のビデオカメラ３および３台のレーザ光照射装置１を、それらのレーザ光照射装置１のレーザビーム光軸とビデオカメラ３のレンズ光軸ＬＣとが１点で交差するように互いに位置決め支持する、図示例では概略円形枠状のブラケット５を支持しており、右の作業腕１３の手首関節２２では、エンドエフェクタとしての図示しない電動ドライバを交換可能に支持している。
【００５６】
さらに、この実施例の台車型作業ロボット１０は、図６に示すように、支柱１６の下端部を脚の代わりに支持する台車２４を具えており、この台車２４は、支柱１６を介して胴部１４を昇降移動させる例えば通常のボールねじ式の図示しない電動アクチュエータと、作業腕１３や胴部１４内に収容された各可動軸用駆動回路の作動を制御する、通常のコンピュータを有する作動制御手段としての図示しないコントロールボックスと、頭部１２に搭載された二台のビデオカメラ１１からの画像（視覚情報）に基づき、机やワークに表示された複数のマークの相対位置から机やワークの３次元座標および向きを認識して上記コントロールボックスに伝達するとともに、手首関節２２に支持された上記実施例の撮像面位置検出装置のビデオカメラ３からの画像に基づき、前述の如くしてレーザ光照射装置１に対する机やワーク等の対象物２の撮像面２ａの距離および向きひいては当該台車型作業ロボット１０の３次元座標系における撮像面２ａの位置および向きを演算で求めて上記コントロールボックスに伝達する、これも通常のコンピュータを有する図示しない視覚制御ボックスと、を搭載している。従って、ここにおける視覚制御ボックスは演算装置４を構成する。
【００５７】
そして、ここにおける台車２４はその下部に、四本の固定脚２５と、各々首振り可能な図示しない四個のキャスタとを有しており、これらのキャスタは、台車２４の下部に平行リンクを介して昇降可能に支持された可動フレームに取付けられて可動フレームとともに昇降可能とされ、可動フレームは、図示しないペダル等の操作により、キャスタが固定脚２５よりも下がって台車２４を床面上で走行可能に支持する下降位置と、キャスタが固定脚２５よりも上がって台車２４が固定脚２５で固定支持される収納位置との間で昇降移動させることができる。また、ここにおける台車２４はその後部に、人手で押し引きできるようにハンドル２６を設けられるとともに、そのハンドル２６に、図示しないスイッチボックスを着脱可能に装着されており、このスイッチボックスには、この実施例の台車型作業ロボット１０を完全に停止させる非常停止スイッチおよび、この実施例の台車型作業ロボット１０を制御系は生かしたまま作動のみ停止させる保護停止スイッチと、上記電動アクチュエータによる胴部１４の昇降スイッチとが設けられている。
【００５８】
従って、この実施例の台車型作業ロボット１０は、手動で任意の作業場所へ移動させて、その場所で作業させることができる。なお、上記実施例の撮像面位置検出装置を除いたこの台車型作業ロボット１０の構成の詳細は、本願出願人が先に特開２００８−２６４８９９号公報および特開２０１０−０６４１９８号公報等にて開示しており、当業者に周知である。
【００５９】
かかる実施例の台車型作業ロボット１０にあっては、例えばワークに対してネジ締め作業を行う場合、上記視覚制御ボックスが、２台のビデオカメラ１１が撮像する画像から、机や、その机上のパレットや、そのパレット上のワークの種類並びに概略３次元位置を、それらの表面に表示された複数のクロスマークの配置からパターンマッチングや写真測量等の方法により計測してコントロールボックスに知らせ、コントロールボックスはそれらの種類および３次元位置に応じて、先ず、ハンド２１とともに上記実施例の撮像面位置検出装置のビデオカメラ３およびレーザ光照射装置１を、パレット上のワークに近づけるように、左の作業腕１３を作動させる。
【００６０】
次いで、ワーク表面に照射された３本のレーザビームの照射点をビデオカメラ３がワークに対し至近距離の位置で撮像し、上記視覚制御ボックスが、前述の如くしてそのワークの３次元位置および向きをその至近距離のデータから高精度に求めてコントロールボックスに知らせ、これによりコントロールボックスが、左の作業腕１３を正確に作動させてハンド２１のフィンガ２１ａでワークを確実に把持して持ち上げる。そしてこの把持によりワークの３次元位置および姿勢が高精度に定まったので、コントロールボックスは次に、右の作業腕１３を正確に作動させて電動ドライバでネジ締めを行い、さらに左の作業腕１３を作動させて、ネジ締めが済んだワークを別の所定のパレット上に移動させ、そこでハンド２１のフィンガ２１ａを解放作動させてそのワークをパレット上に載置する。
【００６１】
従って、この実施例の撮像面位置検出装置によれば、１台の単眼のビデオカメラ３で撮像したワークの１枚の画像からビデオカメラ３に対する対象物２としてのワークの撮像面２ａの３次元位置すなわち距離と向きとを幾何学的演算により検出するので、ワークの撮像面の位置を簡易かつ安価に、しかも短時間で検出することができる。
【００６２】
そして、この実施例の台車型作業ロボット１０によれば、簡易かつ安価な構成により実質的にリアルタイムでワーク等の対象物２の撮像面２ａの位置を求めて、作業腕１３と、その作業腕１３の先端部に設けられたハンド２１や電動ドライバ等のエンドエフェクタとを作動させることができるので、製造コストを高めることなく作業ロボットの作動速度を高めることができる。
【００６３】
しかも、この実施例の台車型作業ロボット１０によれば、撮像面位置検出装置の３台のレーザ光照射装置１と１台のビデオカメラ３との相対的な位置関係がブラケット５によって固定されているので、撮像面位置検出装置を具える作業腕１３が作動停止後に多少振動していてもその振動にほとんど影響されずに撮像面２ａの位置検出を行うことができる。
【００６４】
以上、図示例に基づき説明したが、本発明は上述した実施例に限定されるものでなく、特許請求の範囲の記載範囲内で適宜変更することができるものであり、例えば、この発明の撮像面位置検出装置においては、少なくとも３台のレーザ光照射手段からのレーザビームは、そのうちの少なくとも２台ずつでレーザ光軸が互いに交差するように配置されていれば足りる。また、この発明の撮像面位置検出装置においては、対象物の撮像面がレーザ光軸の交差点より手前にあるか先にあるかが別のカメラ等の手段によって判る場合には、レーザ光照射手段は必ずしも奇数台具えていなくても良い。そしてこの発明の撮像面位置検出装置においては、対象物の撮像面は、所定半径の球殻面等、幾何学的寸法形状が明らかなものであれば、必ずしも平面でなくてもよい。
【００６５】
また、上記実施例の作業ロボットは移動式の台車型作業ロボットとしたが、本発明の作業ロボットは移動式でなく所定位置に固定されるものでも良い。また、上記実施例の作業ロボットはヒューマノイド型の双腕ロボットとしたが、本発明の作業ロボットは、エンドエフェクタを持つ多関節型作業腕とその作動を制御する制御装置とを具える通常の産業用ロボットでも良い。そして、本発明の撮像面位置検出装置の少なくとも３台のレーザ光照射手段と１台の撮像手段とは、作業ロボットの作業腕以外の部位、例えば作業場所の壁面等に設けても良い。
【産業上の利用可能性】
【００６６】
かくしてこの発明の撮像面位置検出装置によれば、１台の単眼の撮像手段で撮像した対象物の１枚の画像から撮像手段に対する対象物の撮像面の位置すなわち距離と向きとを幾何学的演算により検出するので、対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出することができる。
【００６７】
また、この発明の作業ロボットによれば、簡易かつ安価な構成により、実質的にリアルタイムで対象物の撮像面の位置を求めて、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させることができるので、製造コストを高めることなく作業ロボットの作動速度を高めることができる。
【００６８】
しかも、撮像面位置検出装置の少なくとも３台のレーザ光照射手段と撮像手段との位置関係が固定されているので、撮像面位置検出装置を具える頭や腕等の部位が作動停止後に多少振動していても、その振動にほとんど影響されずに撮像面の位置検出を行うことができる。
【符号の説明】
【００６９】
１ レーザ光照射装置
２ 対象物
２ａ 撮像面
３ ビデオカメラ
４ 演算装置
５ ブラケット
５ａ 枠状部
５ｂ 腕部
５ｃ 筒状部
１０ 台車型作業ロボット
１１ ビデオカメラ
１２ 頭部
１３ 作業腕
１３ａ 上腕
１３ｂ 下腕
１４ 胴部
１５ 上半身
１６ 支柱
１７ 首部関節
１８ 胴部関節
１９ 肩関節
２０ 肘関節
２１ ハンド
２１ａ フィンガ
２２ 手首関節
２３ 肩ブラケット
２４ 台車
２５ 固定脚
２６ ハンドル
Ｂ１，Ｂ２，Ｂ３ レーザビーム
ＣＰ ３本のレーザビームとレンズ光軸との交点
Ｉ１，Ｉ２，Ｉ３ 照射点
ＬＣ レンズ光軸
Ｐ１，Ｐ２，Ｐ３，Ｐ４ ピッチ軸線
Ｒ ロール軸線
Ｙ１，Ｙ２，Ｙ３ ヨー軸線
【技術分野】
【０００１】
この発明は、ロボットの頭部やハンドに設けられたビデオカメラ等のカメラで撮像した対象物の撮像面の、撮像したカメラに対する位置すなわち距離と向きとを検出する装置並びに、その装置を具える作業ロボットに関するものである。
【背景技術】
【０００２】
近年、例えば１人ないし数人の作業者が１つの製品を作り上げるセル生産の現場等に、人と協調作業する作業ロボットを投入することが提案されており、かかる作業ロボットとしては従来例えば、下半身に台車を具え、その台車により作業場所内の床上を移動させることで、作業腕を持つ上半身をテーブル上等の作業領域の近傍の作業位置に簡便に配置し得るようにしたものが知られている（特許文献１参照）。
【０００３】
この作業ロボットではまた、頭部に複数台のビデオカメラを具え、それらのビデオカメラでテーブルやその上のワークに設けた複数のマークを複数方向から撮像して、それら複数方向からの複数枚の画像からビデオカメラひいては作業ロボットに対するワークやテーブルの位置すなわち距離と向きとを検出することで、上記のような簡便な配置でもワークの位置を作業ロボットに教示せずにワークに対する作業を行わせ得るようにしている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００７−１１８１７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上述のように、作業ロボットの利用を進めるために視覚技術を取り入れることは、視覚による情報量が他の知覚と比べて圧倒的に多いため極めて有効であり、視覚技術としてはカメラ画像を利用することが簡便である。
【０００６】
しかしながら、カメラ画像は２次元情報であるため、対象物の位置を３次元的に捉えるには複数枚のカメラ画像が必要となる。そして、特に作業ロボットの作動制御等のためにリアルタイム（実時間）で複数枚の画像を得るには、上述のように複数台のビデオカメラを具えることが必須となって、対象物位置検出装置の製造コストひいては作業ロボットの製造コストが嵩むことになるという問題がある。
【０００７】
また、複数枚の画像を処理してそれらの画像相互の関係から２次元情報を３次元情報に変換するには、複雑な処理と短くはない解析時間とを要するという問題もある。
【０００８】
それゆえこの発明は、１台の単眼の撮像手段で撮像した対象物の１枚の画像から撮像手段に対する対象物の撮像面の位置すなわち距離と向きとを検出することで、対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出する装置を提供することを目的としている。
【０００９】
またこの発明は、上述の装置を具える作業ロボットを提供することを目的としている。
【課題を解決するための手段】
【００１０】
この発明は上記課題を有利に解決するものであり、この発明の撮像面位置検出装置は、レーザ光軸が互いに交差するように配置された少なくとも３台のレーザ光照射手段と、前記少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像する１台の単眼すなわち単一光軸の撮像手段と、前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の撮像光軸との位置関係から幾何学的演算により前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する演算手段と、を具えてなるものである。
【００１１】
またこの発明の作業ロボットは、前記撮像手段に対する前記対象物の撮像面の距離と向きとを検出して出力する前記撮像面位置検出装置と、前記出力された撮像面の距離と向きとに基づき、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させる作動制御手段と、を具えてなるものである。
【発明の効果】
【００１２】
上述したこの発明の撮像面位置検出装置にあっては、少なくとも３台のレーザ光照射手段がそれぞれ、レーザ光軸が互いに交差する向きでレーザビームを対象物の表面に照射し、１台の単眼の撮像手段がそれら少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像し、演算手段が、前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の単一の撮像光軸との位置関係から幾何学的演算により、前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する。
【００１３】
従って、この発明の撮像面位置検出装置によれば、１台の単眼の撮像手段で撮像した対象物の１枚の画像から撮像手段に対する対象物の撮像面の位置すなわち距離と向きとを幾何学的演算により検出するので、対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出することができる。
【００１４】
なお、この発明の撮像面位置検出装置においては、前記少なくとも３台のレーザ光照射手段からのレーザビームは、前記撮像手段の撮像光軸上の１点で互いに交差すると好ましい。撮像光軸を基準にしてレーザビームの各照射点の位置を表せるので幾何学的計算をより簡易化できるからでる。
【００１５】
また、この発明の撮像面位置検出装置においては、前記レーザ光照射手段を奇数台具えていると好ましい。対象物の撮像面上のレーザビームの照射点の数が奇数となるため、対象物の撮像面がレーザ光軸の交差点より手前のある場合と先にある場合とで照射点を結んだ図形の向きが逆向きになるので、撮像面の位置をより容易に計算できるからである。
【００１６】
さらに、この発明の撮像面位置検出装置においては、前記対象物の撮像面は平面であると好ましい。対象物の撮像面上のレーザビームの照射点の位置関係から撮像面の位置をより容易に計算できるからである。
【００１７】
一方、上述したこの発明の作業ロボットにあっては、作業ロボットに設けられた撮像面位置検出装置が、その撮像手段に対する対象物の撮像面の距離と向きとを検出して出力し、作動制御手段が、撮像面位置検出装置から出力された前記撮像面の距離と向きとに基づき、その作業ロボットの腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させる。
【００１８】
従って、この発明の作業ロボットによれば、簡易かつ安価な構成により、実質的にリアルタイムで対象物の撮像面の位置を求めて、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させることができるので、製造コストを高めることなく作業ロボットの作動速度を高めることができる。
【００１９】
しかも、撮像面位置検出装置の少なくとも３台のレーザ光照射手段と撮像手段との位置関係が固定されているので、撮像面位置検出装置を具える頭や腕等の部位が作動停止後に多少振動していても、その振動にほとんど影響されずに撮像面の位置検出を行うことができる。
【図面の簡単な説明】
【００２０】
【図１】この発明の撮像面位置検出装置の一実施例の構成を、光学系を斜め後方から見た状態で示す構成図である。
【図２】上記実施例の撮像面位置検出装置の光学系を斜め前方から見た状態で示す斜視図である。
【図３】上記実施例の撮像面位置検出装置の光学系をカメラ画像の向きで見た状態で示す平面図である。
【図４】上記実施例の撮像面位置検出装置の光学系と対象物との幾何学的位置関係を図３中のＡ−Ａ線に沿う断面で示す説明図である。
【図５】上記実施例の撮像面位置検出装置の光学系と対象物との幾何学的位置関係を図４と同様の位置での断面で示す説明図である。
【図６】上記実施例の撮像面位置検出装置を具えた、この発明の作業ロボットの一実施例を示す斜視図である。
【図７】上記実施例の作業ロボットの腕の先端部に具えるハンドおよび上記実施例の撮像面位置検出装置を示す斜視図である。
【図８】図７に示すハンドおよび上記実施例の撮像面位置検出装置を斜め前方から見た状態で示す斜視図である。
【発明を実施するための形態】
【００２１】
以下、この発明の実施の形態を図面に基づく実施例によって詳細に説明する。ここに、図１は、この発明の撮像面位置検出装置の一実施例の構成を、光学系を斜め後方から見た状態で示す構成図、図２は、上記実施例の撮像面位置検出装置の光学系を斜め前方から見た状態で示す斜視図、図３は、上記実施例の撮像面位置検出装置の光学系をカメラ画像の向きで見た状態で示す平面図、そして図４は、上記実施例の撮像面位置検出装置の光学系と対象物との幾何学的位置関係を図３中のＡ−Ａ線に沿う断面で示す説明図である。
【００２２】
この実施例の撮像面位置検出装置は、レーザビーム（光線）の光軸が互いに交差するように配置された３台の、レーザ光照射手段としてのレーザ光照射装置１と、それら３台のレーザ光照射装置１に対する所定位置で、それらのレーザ光照射装置１からレーザビームＢ１，Ｂ２，Ｂ３を照射された対象物２の表面である撮像面２ａを撮像する１台の、単眼の撮像手段としてのビデオカメラ３と、そのビデオカメラ３が対象物（図示例では平板）２の撮像面２ａを撮像した１枚の画像での、３台のレーザ光照射装置１からのレーザビームＢ１，Ｂ２，Ｂ３の３つの照射点Ｉ１，Ｉ２，Ｉ３の位置とビデオカメラ３の撮像光軸としてのレンズ光軸ＬＣとの位置関係から幾何学的演算により、ビデオカメラ３に対する対象物２の撮像面２ａの距離と向きとを求めて出力する、演算手段としての演算装置４と、上記３台のレーザ光照射装置１と１台のビデオカメラ３とを互いに位置決めして固定支持するブラケット５と、を具えてなるものである。
【００２３】
ここで、レーザ光照射装置１としては、例えばレーザダイオードで出力したレーザビームを表示物等に照射し、そのレーザビームの照射点である光点で表示物等を指し示す通常のレーザポインタを用いることができ、またビデオカメラ３としては、例えばＣＣＤ撮像素子で対象物を撮像した画像を電気信号として出力する通常のビデオカメラを用いることができ、そして演算装置４としては、例えばＣＰＵ（中央処理ユニット）、ＲＯＭ（読み出し専用メモリ）、ＲＡＭ（ランダムアクセスメモリ）およびＩ／Ｏ（入出力インターフェース）等を持ち、予めＲＯＭ等に記憶したプログラムに基づき一時記憶にＲＡＭ等を用いながらＣＰＵで演算処理を行ってその結果をＩ／Ｏから出力する通常のマイクロコンピュータを用いることができる。
【００２４】
この実施例におけるブラケット５は、図３に示すように、枠状部５ａによりビデオカメラ３を固定支持するとともに、その枠状部５ａから放射状に突出した３本の腕部５ｂに図示しないボルトで位置調節可能に固定した筒状部５ｃにより３台のレーザ光照射装置１をそれぞれ固定支持している。かかる支持構造により、図４に１台のレーザ光照射装置１について代表で示すように、３台のレーザ光照射装置１からのレーザビームＢ１，Ｂ２，Ｂ３（図４では符号Ｂで示す）の光軸はそれぞれ、ビデオカメラ３のレンズ光軸ＬＣに対しレンズ３ａの位置から距離Ｈｍｍの１点ＣＰで交差するとともに互いに同一の角度θをなし、また図３の平面図で真上から見て示すように、それらのレーザビームＢ１，Ｂ２，Ｂ３のそれぞれの光軸とビデオカメラ３のレンズ光軸ＬＣとを含む、図３の紙面に垂直な平面が３枚存在し、それらの垂直平面はレンズ光軸ＬＣを交線として互いに１２０°の角度で交差している。
【００２５】
上記の位置関係において演算装置４は、ビデオカメラ３が撮像した１枚の画像から、ビデオカメラ３に対する対象物２の、図示例では平面である撮像面２ａの３次元的な距離と向きとを、ビデオカメラ３と撮像面２ａとの幾何学的な関係に基づき、以下の如くして演算で求める。
【００２６】
すなわち、図３に示す向きで撮像したビデオカメラ３の画像は、四角の１コマを１ピクセルとすると水平ピクセル数と垂直ピクセル数とで規定され、指定するピクセルの位置は２次元座標（ｘ，ｙ）にて表記できる。ここで、画像上でのレンズ光軸ＬＣの位置を画像原点（０，０）とするとともに、その画像原点（０，０）からレーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３までの距離をａ１，ａ２，ａ３とすると、
レーザビームＢ１の照射点Ｉ１の位置は（ａ１，０）、
レーザビームＢ２の照射点Ｉ２の位置は（ａ２・cos120°，ａ２・sin120°）、
レーザビームＢ３の照射点Ｉ３の位置は（ａ３・cos240°，ａ３・sin240°）であり、
照射点Ｉ１，Ｉ２，Ｉ３は、画像原点（０，０）から０°、１２０°、２４０°の角度を通る直線上にそれぞれ位置する。
【００２７】
対象物２の撮像面２ａの３次元的な向きを計算すると、図４に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの手前（ビデオカメラ３寄り）に対象物２の撮像面２ａがある場合には、その交点ＣＰを三次元座標系の原点０（０，０，０）、レンズ光軸ＬＣを三次元座標系のｚ軸方向としたときの、撮像面２ａの向きを表す、原点０から撮像面２ａへ向かう単位長さの法線ベクトルＮＵ→（ｌｘ，ｌｙ，ｌｚ）の各座標軸成分は、
照射点Ｉ２の長さ比ｒ２＝ａ２／ａ１、
照射点Ｉ３の長さ比ｒ３＝ａ３／ａ１より、
ｌｘ＝ｋ（１／ｒ２＋１／ｒ３）／（√３tanθ）、
ｌｙ＝ｋ（２−１／ｒ２＋１／ｒ３）／（３tanθ）、
ｌｚ＝ｋ（１−（２−１／ｒ２＋１／ｒ３）／３）
となる。
ここで、ｋ＝１／√｛（１／ｒ２＋１／ｒ３）／（√３tanθ）２＋（２−１／ｒ２＋１／ｒ３）／（３tanθ）２＋（１−（２−１／ｒ２＋１／ｒ３）／３）２｝である。
【００２８】
これについて説明すると、ある点Ａ＝（ｘ０，ｙ０，ｚ０）を通り、その点Ａおよび原点０（０，０，０）を通る直線に垂直な平面を考えた場合に、点Ａからその平面上の任意の点Ｐ（Ｘ，Ｙ，Ｚ）に引いたベクトルＡＰ→は、
ＡＰ→＝（Ｘ−ｘ０，Ｙ−ｙ０，Ｚ−ｚ０）であり、
原点Ｏから点Ａに引いたベクトルＯＡ→は、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、それら互いに直角な２つのベクトルＡＰ→，ＯＡ→の内積は０であることから、
ｘ０（Ｘ−ｘ０）＋ｙ０（Ｙ−ｙ０）＋ｚ０（Ｚ−ｚ０）＝０
が成り立つ。
【００２９】
これをこの実施例の装置に当てはめると、
（１）レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰに設定した原点Ｏ（０，０，０）からレーザビームＢ１上を照射点Ｉ１へ向かう所定長さのベクトルｂ１→を考えると、ｂ１→が３次元座標系のｙ軸と、レンズ光軸ＬＣに一致するｚ軸とを含む平面（先の３枚の垂直平面の１つ）の上にあるとして、
ｂ１→＝（ｂ１ｘ，ｂ１ｙ，ｂ１ｚ）＝（sin0°・tanθ，cos0°・tanθ，１）＝（０，tanθ，１）となり、ベクトルｂ１→の長さは
１／cosθ＝√（１＋tan２θ）=√｛（cos２θ＋sin２θ）／cos２θ｝=√（１／cos２θ）
となる。
上記ベクトルｂ１→を含む直線であるレーザビームＢ１の光軸上の点の座標は、ｔを変数とすると、
ｘ＝sin0°・tanθ・ｔ＝０
ｙ＝cos0°・tanθ・ｔ＝tanθ・ｔ
ｚ＝１・ｔ＝ｔ
となる。
【００３０】
上記点Ａ（ｘ０，ｙ０，ｚ０）が、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を含む平面である撮像面２ａ上にあると考えると、その点Ａおよび原点０（０，０，０）を通る直線は撮像面２ａの法線となるので、上記ベクトルｂ１→を含む直線であるレーザビームＢ１の光軸と、上記法線に垂直な平面である撮像面２ａとの交点であるレーザビームＢ１の照射点Ｉ１を点Ｑ（ｘ，ｙ，ｚ）とすると、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、
ＡＱ→＝（０−ｘ０，tanθ・ｔ−ｙ０，ｔ−ｚ０）である。
これら互いに直角な２つのベクトルＡＱ→，ＯＡ→の内積は０であることから、
ｘ０（０−ｘ０）＋ｙ０（tanθ・ｔ−ｙ０）＋ｚ０（ｔ−ｚ０）＝０
となり、これをｔについて整理すると、
（ｙ０・tanθ＋ｚ０）ｔ＝ｘ０２＋ｙ０２＋ｚｏ２＝Ｌ２
となる。このＬは、原点０（０，０，０）すなわちレーザビームＢ１とレンズ光軸ＬＣとの交点ＣＰから撮像面２ａまでの距離（法線上の長さ）を表す。
また、ｔ＝Ｌ２／（ｙ０・tanθ＋ｚ０）となる。
【００３１】
（２）原点Ｏ（０，０，０）からレーザビームＢ２上を照射点Ｉ２へ向かう所定長さのベクトルｂ２→を考えると、ｂ２→が、３次元座標系のｙ軸とｚ軸とを含む平面に対しｚ軸を中心として反時計回りに１２０°回転した位置にある平面（先の３枚の鉛直平面の他の１つ）の上にあるとして、
ｂ２→＝（ｂ２ｘ，ｂ２ｙ，ｂ２ｚ）＝（sin120°・tanθ，cos120°・tanθ，１）となり、上記ベクトルｂ２→を含む直線であるレーザビームＢ２の光軸上の点の座標は、ｔを変数とすると、
ｘ＝sin120°・tanθ・ｔ
ｙ＝cos120°・tanθ・ｔ
ｚ＝１・ｔ＝ｔ
となる。
【００３２】
上記のように点Ａ（ｘ０，ｙ０，ｚ０）が、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を含む平面である撮像面２ａ上にあると考えると、その点Ａおよび原点０（０，０，０）を通る直線は撮像面２ａの法線となるので、上記ベクトルｂ２→を含む直線であるレーザビームＢ２の光軸と、上記法線に垂直な平面である撮像面２ａとの交点であるレーザビームＢ２の照射点Ｉ２を点Ｒ（ｘ，ｙ，ｚ）とすると、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、
ＡＲ→＝（sin120°・tanθ・ｔ−ｘ０，cos120°・tanθ・ｔ−ｙ０，ｔ−ｚ０）である。
これら互いに直角な２つのベクトルＡＲ→，ＯＡ→の内積は０であることから、
ｘ０（sin120°・tanθ・ｔ−ｘ０）＋ｙ０（cos120°・tanθ・ｔ−ｙ０）＋ｚ０（ｔ−ｚ０）＝０
となり、これをｔについて整理すると、
（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０）ｔ＝Ｌ２
となる。
また、ｔ＝Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０）となる。
【００３３】
（３）原点ＯからレーザビームＢ３上を照射点Ｉ３へ向かう所定長さのベクトルｂ３→を考えると、ｂ３→が、３次元座標系のｙ軸とｚ軸とを含む平面に対しｚ軸を中心として反時計回りに２４０°回転した位置にある平面（先の３枚の鉛直平面の他の１つ）の上にあるとして、
ｂ３→＝（ｂ３ｘ，ｂ３ｙ，ｂ３ｚ）＝（sin240°・tanθ，cos240°・tanθ，１）となり、上記ベクトルｂ３→を含む直線であるレーザビームＢ３の光軸上の点の座標は、ｔを変数とすると、
ｘ＝sin240°・tanθ・ｔ
ｙ＝cos240°・tanθ・ｔ
ｚ＝１・ｔ＝ｔ
となる。
【００３４】
上記のように点Ａ（ｘ０，ｙ０，ｚ０）が、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を含む平面である撮像面２ａ上にあると考えると、その点Ａおよび原点０（０，０，０）を通る直線は撮像面２ａの法線となることから、上記ベクトルｂ３→を含む直線であるレーザビームＢ３の光軸と、上記法線に垂直な平面である撮像面２ａとの交点であるレーザビームＢ２の照射点Ｉ２を点Ｓ（ｘ，ｙ，ｚ）とすると、
ＯＡ→＝（ｘ０，ｙ０，ｚ０）であり、
ＡＳ→＝（sin240°・tanθ・ｔ−ｘ０，cos240°・tanθ・ｔ−ｙ０，ｔ−ｚ０）である。
これら互いに直角な２つのベクトルＡＳ→，ＯＡ→の内積は０であることから、
ｘ０（sin240°・tanθ・ｔ−ｘ０）＋ｙ０（cos240°・tanθ・ｔ−ｙ０）＋ｚ０（ｔ−ｚ０）＝０
となり、これをｔについて整理すると、
（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０）ｔ＝Ｌ２
となる。
また、ｔ＝Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０）となる。
【００３５】
（４）レーザビームＢ１の照射点Ｉ１のｘｙ平面座標は、上記（１）からｘ＝０、ｙ＝tanθ・ｔであり、ｔ＝Ｌ２／（ｙ０・tanθ＋ｚ０）であるので、
（０，tanθ・Ｌ２／（ｙ０・tanθ＋ｚ０））となる。
またレーザビームＢ２の照射点Ｉ２のｘｙ平面座標は、上記（２）からｘ＝sin120°・tanθ・ｔ、ｙ＝cos120°・tanθ・ｔであり、ｔ＝Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０）であるので、
（sin120°・tanθ・Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０），cos120°・tanθ・Ｌ２／（ｘ０・sin120°・tanθ＋ｙ０・cos120°・tanθ＋ｚ０））となる。
さらにレーザビームＢ３の照射点Ｉ３のｘｙ平面座標は、上記（３）からｘ＝sin240°・tanθ・ｔ、ｙ＝cos240°・tanθ・ｔであり、ｔ＝Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０）であるので、
（sin240°・tanθ・Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０），cos240°・tanθ・Ｌ２／（ｘ０・sin240°・tanθ＋ｙ０・cos240°・tanθ＋ｚ０））となる。
【００３６】
（５）撮像面２ａの向きを表す、原点０から撮像面２ａに至る法線ベクトルＮＬ→は（Ｌｘ，Ｌｙ，Ｌｚ）であるので、ｘ０＝Ｌｘ、ｙ０＝Ｌｙ、ｚ０＝Ｌｚであり、これを用いると、
レーザビームＢ１の照射点Ｉ１のｘｙ平面座標は、
（０，tanθ・Ｌ２／（Ｌｙ・tanθ＋Ｌｚ））となる。
またレーザビームＢ２の照射点Ｉ２のｘｙ平面座標は、sin120°＝√３／２，cos120°＝−１／２であるので、
（√３／２・tanθ・Ｌ２／（（√３・Ｌｘ−Ｌｙ）／２・tanθ＋Ｌｚ），−１／２・tanθ・Ｌ２／（（√３・Ｌｘ−Ｌｙ）／２・tanθ＋Ｌｚ））となる。
さらにレーザビームＢ３の照射点Ｉ３のｘｙ平面座標は、sin240°＝−√３／２，cos240°＝−１／２であるので、
（√３／２・tanθ・Ｌ２／（（√３・Ｌｘ＋Ｌｙ）／２・tanθ−Ｌｚ），１／２・tanθ・Ｌ２／（（√３・Ｌｘ＋Ｌｙ）／２・tanθ−Ｌｚ））となる。
【００３７】
（６）レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３の、ｘｙ平面座標系における原点（０，０）からの長さａ１，ａ２，ａ３は、単位長さの法線ベクトルＮＵ→（ｌｘ，ｌｙ，ｌｚ）（但しｌｘ２＋ｌｙ２＋ｌｚ２＝１）を用いて、Ｌｘ／Ｌ＝ｌｘ，Ｌｙ／Ｌ＝ｌｙ，Ｌｚ／Ｌ＝ｌｚとすると、
ａ１＝tanθ・Ｌ／（Ｌｙ／Ｌ・tanθ＋Ｌｚ／Ｌ）＝tanθ・Ｌ／（Ｌｙ／Ｌ・tanθ＋Ｌｚ／Ｌ）＝tanθ・Ｌ／（ｌｙ・tanθ＋ｌｚ）
ａ２＝tanθ・Ｌ／（（√３Ｌｘ／Ｌ−Ｌｙ／Ｌ）／２・tanθ＋Ｌｚ／Ｌ）＝tanθ・Ｌ／（（√３ｌｘ−ｌｙ）／２・tanθ＋ｌｚ）
ａ３＝tanθ・Ｌ／（（√３Ｌｘ／Ｌ＋Ｌｙ／Ｌ）／２・tanθ−Ｌｚ／Ｌ）＝tanθ・Ｌ／（（√３ｌｘ＋ｌｙ）／２・tanθ−ｌｚ）
となる。
【００３８】
（７）ここで、ａ１，ａ２，ａ３についての撮像面２ａの傾きに関する補正係数ｋを、
１≡ｋ・１／（ｌｙ・tanθ＋ｌｚ）と設定すると、
ｌｙ・tanθ＋ｌｚ＝ｋ
従って、ｌｚ＝ｋ−ｌｙ・tanθ
長さ比ｒ２＝ａ２／ａ１とすると、
（ｌｙ・tanθ＋ｌｚ）／（（√３ｌｘ−ｌｙ）／２・tanθ＋ｌｚ）＝ｒ２
これに上記のｌｚの式を入れると、
ｒ２・（（√３ｌｘ−ｌｙ）／２・tanθ＋ｌｚ）＝ｋ
さらに上記のｌｚの式を入れると、
ｒ２・（（√３ｌｘ−ｌｙ）／２・tanθ＋ｋ−ｌｙ・tanθ）＝ｋ
従って、ｒ２・（（√３ｌｘ−３ｌｙ）／２・tanθ＋ｋ）＝ｋとなる。
また、長さ比ｒ３＝ａ３／ａ１とすると、
（ｌｙ・tanθ＋ｌｚ）／（（√３ｌｘ＋ｌｙ）／２・tanθ−ｌｚ）＝ｒ３
これに上記のｌｚの式を入れると、
ｒ３・（（√３ｌｘ＋ｌｙ）／２・tanθ−ｌｚ）＝ｋ
さらに上記のｌｚの式を入れると、
ｒ３・（（√３ｌｘ＋ｌｙ）／２・tanθ−ｋ＋ｌｙ・tanθ）＝ｋ
従って、ｒ３・（（√３ｌｘ＋３ｌｙ）／２・tanθ−ｋ）＝ｋとなる。
【００３９】
これらから、
（√３ｌｘ−３ｌｙ）／２・tanθ＋ｋ−ｋ／ｒ２＝０
（√３ｌｘ＋３ｌｙ）／２・tanθ−ｋ−ｋ／ｒ３＝０
上記両式を加算すると、
√３ｌｘ・tanθ−ｋ／ｒ２−ｋ／ｒ３＝０
これより、ｌｘ＝ｋ（１／ｒ２＋１／ｒ３）／（√３tanθ）となる。
両式を減算すると、
ｋ（２−１／ｒ２＋１／ｒ３）＝３ｌｙ・tanθ
これより、ｌｙ＝ｋ（２−１／ｒ２＋１／ｒ３）／（３tanθ）となる。
さらに、上記のｌｚの式とｌｙより、
ｌｚ＝ｋ−ｌｙ・tanθ＝ｋ−｛ｋ（２−１／ｒ２＋１／ｒ３）／（３tanθ）｝・tanθ
＝ｋ−ｋ（２−１／ｒ２＋１／ｒ３）／３となる。
ここで、ｌｘ２＋ｌｙ２＋ｌｚ２＝１であるので、上記のｌｘ，ｌｙ，ｌｚを代入して、
ｋ２｛（１／ｒ２＋１／ｒ３）／（√３tanθ）２＋（２−１／ｒ２＋１／ｒ３）／（３tanθ）２＋（１−（２−１／ｒ２＋１／ｒ３）／３）２｝＝１
従って、ｋ＝１／√｛（１／ｒ２＋１／ｒ３）／（√３tanθ）２＋（２−１／ｒ２＋１／ｒ３）／（３tanθ）２＋（１−（２−１／ｒ２＋１／ｒ３）／３）２｝となる。
【００４０】
すなわち、撮像面２ａの傾きを表すｌｘ，ｌｙ，ｌｚおよび撮像面２ａの傾きに関する補正係数ｋは何れも、画像におけるａ１に対するａ２，ａ３の長さ比ｒ２，ｒ３と、あらかじめ定めたカメラ光軸ＬＣに対するレーザビームの角度θとから求めることができる。
【００４１】
なお、図５に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの先に対象物２の撮像面２ａがある場合にも、同様にして、撮像面２ａの傾きを表すｌｘ，ｌｙ，ｌｚおよび撮像面２ａの傾きに関する補正係数ｋを何れも、画像におけるａ１に対するａ２，ａ３の長さ比ｒ２，ｒ３と、あらかじめ定めたカメラ光軸ＬＣに対するレーザビームの角度θとから求めることができる。
【００４２】
次に、ビデオカメラ３のレンズ３ａから対象物２の撮像面２ａまでのカメラ光軸ＬＣ上での距離Ｄを計算すると、上述のように、図３に示す向きで撮像したビデオカメラ３の画像は、四角の１コマを１ピクセルとすると水平ピクセル数と垂直ピクセル数とで規定され、指定するピクセルの位置は２次元座標（ｘ，ｙ）にて表記できる。
ここで、既知の１ピクセルのサイズをｓ（ｍｍ）、画像上で求まるカメラ光軸ＬＣからレーザビームＢ１の照射点Ｉ１までのピクセル数をｎｐｉｃとすると、照射点Ｉ１の結像サイズＢｓは、
Ｂｓ＝ｓ・ｎｐｉｃ
で求まる。
そして、既知の焦点距離ｆ（ｍｍ）より結像画角αは、図４および図５に示すように、
ｔａｎα＝Ｂｓ／ｆ＝ａ／Ｄ
で求まる。
【００４３】
レーザビームＢ１において、撮像面２ａ上の照射点Ｉ１とカメラ光軸ＬＣとの距離ａ’（ａ’＝ａ１）は、図４に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの手前に対象物２の撮像面２ａがある場合（Ｄ＜Ｈ：但しＨは、ビデオカメラ３のレンズ３ａから、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とカメラ光軸ＬＣとの交点ＣＰまでの距離を表す）には、
ａ＝ａ’（１＋ｔａｎεｔａｎα）＝Ｄｔａｎα
で表される。但し、ｔａｎε＝Ｌｘ／Ｌｚ＝−ｌｘ／ｌｚである。
両片にｔａｎεを乗じると
ａ’（１＋ｔａｎεｔａｎα）ｔａｎε＝Ｄｔａｎαｔａｎε
従って、
ａ’ｔａｎε＝Ｄｔａｎαｔａｎε／（１＋ｔａｎεｔａｎα）
ここで、ｅ＝ｔａｎαｔａｎε／（１＋ｔａｎεｔａｎα）とすると、
ａ’ｔａｎε＝Ｄｅ
となる。
【００４４】
撮像面２ａ上の照射点Ｉ１の、レンズ３ａからのカメラ光軸ＬＣ上での距離は、図４に示すように、
Ｄ−ａ’ｔａｎε
であり、
一方、撮像面２ａ上の照射点Ｉ１の、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とレンズ光軸ＬＣとの交点ＣＰからのカメラ光軸ＬＣ上での距離は、
Ｌ／ｃｏｓζ＋ａ’ｔａｎε
である。但し、ｔａｎζ＝√（ｌｘ２＋ｌｙ２）／ｌｚである。
次に、ａ’と上記αおよび既知のレーザビーム角θとの関係を求めると、図４に示すように、
ａ’／（Ｄ−ａ’ｔａｎε）＝ｔａｎα
ａ’／（Ｌ／ｃｏｓζ＋ａ’ｔａｎε）＝ｔａｎθ
であり、これにより、
（Ｄ−ａ’ｔａｎε）ｔａｎα＝（Ｌ／ｃｏｓζ＋ａ’ｔａｎε）ｔａｎθ
ここで上記ａ’ｔａｎε＝Ｄｅを用いると、
（Ｄ−Ｄｅ）ｔａｎα＝（Ｌ／ｃｏｓζ＋Ｄｅ）ｔａｎθ
【００４５】
さらに、レンズ３ａから、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とレンズ光軸ＬＣとの交点ＣＰまでの距離Ｈは、図４に示すように、
Ｄ＋Ｌ／ｃｏｓζ＝Ｈ
従って、
Ｌ／ｃｏｓζ＝Ｈ−Ｄ
であり、先のａ’とαおよびθとの関係式に上記の式を代入してＬ／ｃｏｓζを消去すると、
（Ｄ−Ｄｅ）ｔａｎα＝（Ｈ−Ｄ＋Ｄｅ）ｔａｎθ
（Ｄ−Ｄｅ）で纏めると、
（Ｄ−Ｄｅ）（ｔａｎα＋ｔａｎθ）＝Ｈｔａｎθ
従って、
Ｄ（１−ｅ）（ｔａｎα＋ｔａｎθ）＝Ｈｔａｎθ
よって、ビデオカメラ３のレンズ３ａから対象物２の撮像面２ａまでのカメラ光軸ＬＣ上での距離Ｄは、
Ｄ＝Ｈｔａｎθ／｛（１−ｅ）（ｔａｎα＋ｔａｎθ）｝
となる。但し、ｅ＝ｔａｎαｔａｎε／（１＋ｔａｎεｔａｎα）である。
【００４６】
一方、図５に示すように、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの先に対象物２の撮像面２ａがある場合（Ｄ＞Ｈ：但しＨは、ビデオカメラ３のレンズ３ａから、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とカメラ光軸ＬＣとの交点ＣＰまでの距離を表す）には、
Ｌ／ｃｏｓζ＝Ｄ−Ｈ
であるから、レーザビームＢ１において、撮像面２ａ上の照射点Ｉ１とカメラ光軸ＬＣとの距離ａ’（ａ’＝ａ１）は、
ａ＝ａ’（１−ｔａｎεｔａｎα）＝Ｄｔａｎα
であり、ここで、ｇ＝ｔａｎαｔａｎε／（１−ｔａｎεｔａｎα）とすると、
ａ’ｔａｎε＝Ｄｇ
となる。
【００４７】
次に、Ｄ＜Ｈの場合と同様にしてａ’とαおよびθとの関係を求めると、図５に示すように、
ａ’／（Ｄ＋ａ’ｔａｎε）＝ｔａｎα
ａ’／（Ｄ−Ｈ＋ａ’ｔａｎε）＝ｔａｎθ
であり、これにより、
（Ｄ＋ａ’ｔａｎε）ｔａｎα＝（Ｄ−Ｈ＋ａ’ｔａｎε）ｔａｎθ
ここで上記ａ’ｔａｎε＝Ｄｇを用いると、
（Ｄ＋Ｄｇ）ｔａｎα＝（Ｄ−Ｈ＋Ｄｇ）ｔａｎθ
Ｄで纏めると、
Ｄ（１＋ｇ）ｔａｎα＝Ｄ（１−ｇ）ｔａｎθ−Ｈｔａｎθ
従って、
Ｄ｛（１−ｇ）ｔａｎθ−（１＋ｇ）ｔａｎα｝＝Ｈｔａｎθ
よって、ビデオカメラ３のレンズ３ａから対象物２の撮像面２ａまでのカメラ光軸ＬＣ上での距離Ｄは、
Ｄ＝Ｈｔａｎθ／｛（１−ｇ）ｔａｎθ−（１＋ｇ）ｔａｎα｝
＝Ｈｔａｎθ／｛ｔａｎθ−ｔａｎα−ｇ（ｔａｎθ＋ｔａｎα）｝
となる。但し、ｇ＝ｔａｎαｔａｎε／（１−ｔａｎεｔａｎα）である。
【００４８】
なお、対象物２の撮像面２ａが、レーザビームＢ１，Ｂ２，Ｂ３の３本の光軸とビデオカメラ３のレンズ光軸ＬＣとの交点ＣＰの手前にあるか先にあるかの判別は、ビデオカメラ３の画像における、レーザビームＢ１，Ｂ２，Ｂ３の照射点Ｉ１，Ｉ２，Ｉ３を結んだ三角形の向き（図３では、交点ＣＰの手前にあるので三角形の頂点が右に向いており、交点ＣＰの先にある場合は三角形の頂点が左に向く）を画像処理で識別すれば、容易に行うことができる。
【００４９】
そしてこのことは、レーザビームの本数が３本でなくても、その本数が奇数であるか、あるいはレーザビームの照射点を結んだ図形が特定の線に対して線対称（例えば正方形や長方形等）でないものであれば、同様にして成立する。
【００５０】
図６は、上記実施例の撮像面位置検出装置を具えた、この発明の作業ロボットの一実施例としての台車型作業ロボットを示す斜視図、図７は、上記実施例の台車型作業ロボットの腕の先端部に具えるハンドおよび上記実施例の撮像面位置検出装置を示す斜視図、そして図８は、図７に示すハンドおよび上記実施例の撮像面位置検出装置を斜め前方から見た状態で示す斜視図である。
【００５１】
この実施例の台車型作業ロボット１０は、撮像手段としての２台のテレビカメラ１１を有する頭部１２と、２本の作業腕１３と、それらの作業腕１３を支持する胴部１４とを有する上半身１５と、その上半身１５の胴部１４の上端部から下方へ延在する支持部材としての支柱１６とを具えている。２台のテレビカメラ１１は互いに略同一方向へ向いて一緒に頭部１２に固定され、その頭部１２は、頭部１２を水平なピッチ軸線Ｐ１周りに揺動させる首ピッチ軸駆動機構と、その首ピッチ軸駆動機構を鉛直なヨー軸線Ｙ１周りに回動させる首ヨー軸駆動機構とを有する２自由度の首部関節１７を介して胴部１４の上端部に取り付けられている。そして胴部１４の下端部は、ヨー軸線Ｙ１周りに胴部１４を回動させる胴部ヨー軸駆動機構を有する１自由度の胴部関節１８を介して支柱１６の下端部に取り付けられ、これにより支柱１６はその上端部で上半身１５を支持している。
【００５２】
また、２本の作業腕１３は胴部１４の左右両側にそれぞれ設けられ、各作業腕１３は、胴部１４と作業腕１３の上腕１３ａとの間の肩関節１９と、上腕１３ａと下腕１３ｂとの間の肘関節２０と、下腕１３ｂと図６〜８に示すハンド２１や図示しない電動ドライバ等のエンドエフェクタとの間の手首関節２２とを有しており、肩関節１９は、図６に示すように、胴部１４の左右に突出した肩ブラケット２３にそれぞれ配置され、胴部１４に対し作業腕１３の全体をヨー軸線Ｙ２周りに相対的に左右に回動させる肩ヨー軸駆動機構と、胴部１４に対し、そのヨー軸線Ｙ２と直交するピッチ軸線Ｐ２周りに作業腕１３の全体を相対的に前後に傾動させる肩ピッチ軸駆動機構とを有している。
【００５３】
ここで、肩ブラケット２３の肩ヨー軸駆動機構支持面は、鉛直なヨー軸線Ｙ１に対して斜め下方に１５度傾いて配置されており、このため各ヨー軸線Ｙ２は、胴部１４の上下方向に対して１５度傾いて、下方へ行くほど胴部１４に近くなるように延在し、それに伴い、各ヨー軸線Ｙ２と直交する各ピッチ軸線Ｐ２も、床面に対して１５度傾いている。そして作業腕１３の上腕１３ａは、下に下げた状態で肘関節２０に近い部分ほど胴部１４に近くなるように傾いて延在している。この構成によれば、この実施例の台車型作業ロボット１０がその横に位置する作業者と協調あるいは協働して作業を行う場合に、作業者の横に位置する肘関節２０が、作業者から離れており、しかも横に動かないので、作業者が作業腕１３と干渉する可能性を小さくすることができる。
【００５４】
さらに、各肘関節２０は、上腕１３ａに対し下腕１３ｂを、ピッチ軸線Ｐ２に平行なピッチ軸線Ｐ３周りに上下に傾動させる肘ピッチ軸駆動機構を有し、また、各手首関節２２は、下腕１３ｂに対してハンド２１や電動ドライバ等のエンドエフェクタをピッチ軸線Ｐ４周りに相対的に上下に傾動させる手首ピッチ軸駆動機構と、そのエンドエフェクタをピッチ軸線Ｐ４と直交するヨー軸線Ｙ３周りに相対的に左右に回動させる手首ヨー軸駆動機構と、下腕１３ｂに対して手首ピッチ軸駆動機構および手首ヨー軸駆動機構をその下腕１３ｂに沿って延在するロール軸線Ｒ周りにねじる手首ロール軸駆動機構とを有している。この結果として各作業腕１３は６自由度を有し、これらピッチ軸、ヨー軸およびロール軸駆動機構の軸配置により、２本の作業腕１３は特異点がなく自由な姿勢を作ることができる。なお、上記各関節のピッチ軸、ヨー軸およびロール軸の軸駆動機構は各々、周知のように例えばサーボモータ等のモータの出力回転を例えば商品名ハーモニックドライブ等の減速機で減速して出力する、逆入力可能な回動機構で構成されている。
【００５５】
そして、この実施例の台車型作業ロボット１０は、図６〜８に示すように、左の作業腕１３の手首関節２２で、エンドエフェクタとしてのハンド２１とともに、上記実施例の撮像面位置検出装置の１台のビデオカメラ３および３台のレーザ光照射装置１を、それらのレーザ光照射装置１のレーザビーム光軸とビデオカメラ３のレンズ光軸ＬＣとが１点で交差するように互いに位置決め支持する、図示例では概略円形枠状のブラケット５を支持しており、右の作業腕１３の手首関節２２では、エンドエフェクタとしての図示しない電動ドライバを交換可能に支持している。
【００５６】
さらに、この実施例の台車型作業ロボット１０は、図６に示すように、支柱１６の下端部を脚の代わりに支持する台車２４を具えており、この台車２４は、支柱１６を介して胴部１４を昇降移動させる例えば通常のボールねじ式の図示しない電動アクチュエータと、作業腕１３や胴部１４内に収容された各可動軸用駆動回路の作動を制御する、通常のコンピュータを有する作動制御手段としての図示しないコントロールボックスと、頭部１２に搭載された二台のビデオカメラ１１からの画像（視覚情報）に基づき、机やワークに表示された複数のマークの相対位置から机やワークの３次元座標および向きを認識して上記コントロールボックスに伝達するとともに、手首関節２２に支持された上記実施例の撮像面位置検出装置のビデオカメラ３からの画像に基づき、前述の如くしてレーザ光照射装置１に対する机やワーク等の対象物２の撮像面２ａの距離および向きひいては当該台車型作業ロボット１０の３次元座標系における撮像面２ａの位置および向きを演算で求めて上記コントロールボックスに伝達する、これも通常のコンピュータを有する図示しない視覚制御ボックスと、を搭載している。従って、ここにおける視覚制御ボックスは演算装置４を構成する。
【００５７】
そして、ここにおける台車２４はその下部に、四本の固定脚２５と、各々首振り可能な図示しない四個のキャスタとを有しており、これらのキャスタは、台車２４の下部に平行リンクを介して昇降可能に支持された可動フレームに取付けられて可動フレームとともに昇降可能とされ、可動フレームは、図示しないペダル等の操作により、キャスタが固定脚２５よりも下がって台車２４を床面上で走行可能に支持する下降位置と、キャスタが固定脚２５よりも上がって台車２４が固定脚２５で固定支持される収納位置との間で昇降移動させることができる。また、ここにおける台車２４はその後部に、人手で押し引きできるようにハンドル２６を設けられるとともに、そのハンドル２６に、図示しないスイッチボックスを着脱可能に装着されており、このスイッチボックスには、この実施例の台車型作業ロボット１０を完全に停止させる非常停止スイッチおよび、この実施例の台車型作業ロボット１０を制御系は生かしたまま作動のみ停止させる保護停止スイッチと、上記電動アクチュエータによる胴部１４の昇降スイッチとが設けられている。
【００５８】
従って、この実施例の台車型作業ロボット１０は、手動で任意の作業場所へ移動させて、その場所で作業させることができる。なお、上記実施例の撮像面位置検出装置を除いたこの台車型作業ロボット１０の構成の詳細は、本願出願人が先に特開２００８−２６４８９９号公報および特開２０１０−０６４１９８号公報等にて開示しており、当業者に周知である。
【００５９】
かかる実施例の台車型作業ロボット１０にあっては、例えばワークに対してネジ締め作業を行う場合、上記視覚制御ボックスが、２台のビデオカメラ１１が撮像する画像から、机や、その机上のパレットや、そのパレット上のワークの種類並びに概略３次元位置を、それらの表面に表示された複数のクロスマークの配置からパターンマッチングや写真測量等の方法により計測してコントロールボックスに知らせ、コントロールボックスはそれらの種類および３次元位置に応じて、先ず、ハンド２１とともに上記実施例の撮像面位置検出装置のビデオカメラ３およびレーザ光照射装置１を、パレット上のワークに近づけるように、左の作業腕１３を作動させる。
【００６０】
次いで、ワーク表面に照射された３本のレーザビームの照射点をビデオカメラ３がワークに対し至近距離の位置で撮像し、上記視覚制御ボックスが、前述の如くしてそのワークの３次元位置および向きをその至近距離のデータから高精度に求めてコントロールボックスに知らせ、これによりコントロールボックスが、左の作業腕１３を正確に作動させてハンド２１のフィンガ２１ａでワークを確実に把持して持ち上げる。そしてこの把持によりワークの３次元位置および姿勢が高精度に定まったので、コントロールボックスは次に、右の作業腕１３を正確に作動させて電動ドライバでネジ締めを行い、さらに左の作業腕１３を作動させて、ネジ締めが済んだワークを別の所定のパレット上に移動させ、そこでハンド２１のフィンガ２１ａを解放作動させてそのワークをパレット上に載置する。
【００６１】
従って、この実施例の撮像面位置検出装置によれば、１台の単眼のビデオカメラ３で撮像したワークの１枚の画像からビデオカメラ３に対する対象物２としてのワークの撮像面２ａの３次元位置すなわち距離と向きとを幾何学的演算により検出するので、ワークの撮像面の位置を簡易かつ安価に、しかも短時間で検出することができる。
【００６２】
そして、この実施例の台車型作業ロボット１０によれば、簡易かつ安価な構成により実質的にリアルタイムでワーク等の対象物２の撮像面２ａの位置を求めて、作業腕１３と、その作業腕１３の先端部に設けられたハンド２１や電動ドライバ等のエンドエフェクタとを作動させることができるので、製造コストを高めることなく作業ロボットの作動速度を高めることができる。
【００６３】
しかも、この実施例の台車型作業ロボット１０によれば、撮像面位置検出装置の３台のレーザ光照射装置１と１台のビデオカメラ３との相対的な位置関係がブラケット５によって固定されているので、撮像面位置検出装置を具える作業腕１３が作動停止後に多少振動していてもその振動にほとんど影響されずに撮像面２ａの位置検出を行うことができる。
【００６４】
以上、図示例に基づき説明したが、本発明は上述した実施例に限定されるものでなく、特許請求の範囲の記載範囲内で適宜変更することができるものであり、例えば、この発明の撮像面位置検出装置においては、少なくとも３台のレーザ光照射手段からのレーザビームは、そのうちの少なくとも２台ずつでレーザ光軸が互いに交差するように配置されていれば足りる。また、この発明の撮像面位置検出装置においては、対象物の撮像面がレーザ光軸の交差点より手前にあるか先にあるかが別のカメラ等の手段によって判る場合には、レーザ光照射手段は必ずしも奇数台具えていなくても良い。そしてこの発明の撮像面位置検出装置においては、対象物の撮像面は、所定半径の球殻面等、幾何学的寸法形状が明らかなものであれば、必ずしも平面でなくてもよい。
【００６５】
また、上記実施例の作業ロボットは移動式の台車型作業ロボットとしたが、本発明の作業ロボットは移動式でなく所定位置に固定されるものでも良い。また、上記実施例の作業ロボットはヒューマノイド型の双腕ロボットとしたが、本発明の作業ロボットは、エンドエフェクタを持つ多関節型作業腕とその作動を制御する制御装置とを具える通常の産業用ロボットでも良い。そして、本発明の撮像面位置検出装置の少なくとも３台のレーザ光照射手段と１台の撮像手段とは、作業ロボットの作業腕以外の部位、例えば作業場所の壁面等に設けても良い。
【産業上の利用可能性】
【００６６】
かくしてこの発明の撮像面位置検出装置によれば、１台の単眼の撮像手段で撮像した対象物の１枚の画像から撮像手段に対する対象物の撮像面の位置すなわち距離と向きとを幾何学的演算により検出するので、対象物の撮像面の位置を簡易かつ安価に、しかも短時間で検出することができる。
【００６７】
また、この発明の作業ロボットによれば、簡易かつ安価な構成により、実質的にリアルタイムで対象物の撮像面の位置を求めて、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させることができるので、製造コストを高めることなく作業ロボットの作動速度を高めることができる。
【００６８】
しかも、撮像面位置検出装置の少なくとも３台のレーザ光照射手段と撮像手段との位置関係が固定されているので、撮像面位置検出装置を具える頭や腕等の部位が作動停止後に多少振動していても、その振動にほとんど影響されずに撮像面の位置検出を行うことができる。
【符号の説明】
【００６９】
１ レーザ光照射装置
２ 対象物
２ａ 撮像面
３ ビデオカメラ
４ 演算装置
５ ブラケット
５ａ 枠状部
５ｂ 腕部
５ｃ 筒状部
１０ 台車型作業ロボット
１１ ビデオカメラ
１２ 頭部
１３ 作業腕
１３ａ 上腕
１３ｂ 下腕
１４ 胴部
１５ 上半身
１６ 支柱
１７ 首部関節
１８ 胴部関節
１９ 肩関節
２０ 肘関節
２１ ハンド
２１ａ フィンガ
２２ 手首関節
２３ 肩ブラケット
２４ 台車
２５ 固定脚
２６ ハンドル
Ｂ１，Ｂ２，Ｂ３ レーザビーム
ＣＰ ３本のレーザビームとレンズ光軸との交点
Ｉ１，Ｉ２，Ｉ３ 照射点
ＬＣ レンズ光軸
Ｐ１，Ｐ２，Ｐ３，Ｐ４ ピッチ軸線
Ｒ ロール軸線
Ｙ１，Ｙ２，Ｙ３ ヨー軸線
【特許請求の範囲】
【請求項１】
レーザ光軸が互いに交差するように配置された少なくとも３台のレーザ光照射手段と、
前記少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像する１台の単眼の撮像手段と、
前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の撮像光軸との位置関係から幾何学的演算により前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する演算手段と、
を具えてなる、撮像面位置検出装置。
【請求項２】
前記少なくとも３台のレーザ光照射手段からのレーザビームは前記撮像手段の撮像光軸上の１点で互いに交差する、請求項１記載の撮像面位置検出装置。
【請求項３】
前記レーザ光照射手段を奇数台具えている、請求項１または２記載の撮像面位置検出装置。
【請求項４】
前記対象物の撮像面は平面である、請求項１から３までの何れか１項記載の撮像面位置検出装置。
【請求項５】
前記撮像手段に対する前記対象物の撮像面の距離と向きとを検出して出力する請求項１から４までの何れか１項記載の撮像面位置検出装置と、
前記出力された撮像面の距離と向きとに基づき、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させる作動制御手段と、
を具えてなる作業ロボット。
【請求項１】
レーザ光軸が互いに交差するように配置された少なくとも３台のレーザ光照射手段と、
前記少なくとも３台のレーザ光照射手段に対する所定位置で、それらのレーザ光照射手段からレーザビームを照射された対象物の表面を撮像する１台の単眼の撮像手段と、
前記撮像手段が前記対象物の撮像面を撮像した１枚の画像での前記レーザビームの少なくとも３つの照射点の位置と前記撮像手段の撮像光軸との位置関係から幾何学的演算により前記撮像手段に対する前記対象物の撮像面の距離と向きとを求めて出力する演算手段と、
を具えてなる、撮像面位置検出装置。
【請求項２】
前記少なくとも３台のレーザ光照射手段からのレーザビームは前記撮像手段の撮像光軸上の１点で互いに交差する、請求項１記載の撮像面位置検出装置。
【請求項３】
前記レーザ光照射手段を奇数台具えている、請求項１または２記載の撮像面位置検出装置。
【請求項４】
前記対象物の撮像面は平面である、請求項１から３までの何れか１項記載の撮像面位置検出装置。
【請求項５】
前記撮像手段に対する前記対象物の撮像面の距離と向きとを検出して出力する請求項１から４までの何れか１項記載の撮像面位置検出装置と、
前記出力された撮像面の距離と向きとに基づき、腕と、その腕の先端部に設けられたエンドエフェクタとの少なくとも一方を作動させる作動制御手段と、
を具えてなる作業ロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−２５７２６７（Ｐ２０１１−２５７２６７Ａ）
【公開日】平成２３年１２月２２日（２０１１．１２．２２）
【国際特許分類】
【出願番号】特願２０１０−１３２０４９（Ｐ２０１０−１３２０４９）
【出願日】平成２２年６月９日（２０１０．６．９）
【出願人】（５９１２１０６００）川田工業株式会社 (57)
【Ｆターム（参考）】
【公開日】平成２３年１２月２２日（２０１１．１２．２２）
【国際特許分類】
【出願日】平成２２年６月９日（２０１０．６．９）
【出願人】（５９１２１０６００）川田工業株式会社 (57)
【Ｆターム（参考）】
[ Back to top ]